diff --git a/.dockerignore b/.dockerignore index 2d44942185..231d1ed0cc 100644 --- a/.dockerignore +++ b/.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/ diff --git a/.github/workflows/ci_pr.yml b/.github/workflows/ci_pr.yml index db0e990aef..5a3306cc96 100644 --- a/.github/workflows/ci_pr.yml +++ b/.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 diff --git a/.github/workflows/ci_release.yml b/.github/workflows/ci_release.yml index 449de2001a..b0e97618da 100644 --- a/.github/workflows/ci_release.yml +++ b/.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 diff --git a/README.md b/README.md index 747c431e9b..0fb8c70fa0 100644 --- a/README.md +++ b/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), diff --git a/RELEASE.md b/RELEASE.md index 6df6b794e7..6d37518387 100644 --- a/RELEASE.md +++ b/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 diff --git a/astrobee.doxyfile b/astrobee.doxyfile index 0876902677..d2ae0d7d60 100644 --- a/astrobee.doxyfile +++ b/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 diff --git a/astrobee/CMakeLists.txt b/astrobee/CMakeLists.txt index bb6271bb59..574d4d1eed 100644 --- a/astrobee/CMakeLists.txt +++ b/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) diff --git a/astrobee/config/cameras.config b/astrobee/config/cameras.config index 05ac92d952..0d6dcb3aed 100644 --- a/astrobee/config/cameras.config +++ b/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 } } } diff --git a/astrobee/config/geometry.config b/astrobee/config/geometry.config index 02cc1e8061..21e5a88350 100644 --- a/astrobee/config/geometry.config +++ b/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)); diff --git a/astrobee/config/graph_localizer.config b/astrobee/config/graph_localizer.config index 897c5647fc..e0e0912015 100644 --- a/astrobee/config/graph_localizer.config +++ b/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 diff --git a/astrobee/config/localization/depth_odometry.config b/astrobee/config/localization/depth_odometry.config new file mode 100644 index 0000000000..be72977dbb --- /dev/null +++ b/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 diff --git a/astrobee/config/management/cpu_mem_monitor.config b/astrobee/config/management/cpu_mem_monitor.config index b172b91c2a..3bdae428a1 100644 --- a/astrobee/config/management/cpu_mem_monitor.config +++ b/astrobee/config/management/cpu_mem_monitor.config @@ -18,6 +18,7 @@ -- frequency to check and publish cpu stats llp = { + update_pid_hz = 0.1, update_freq_hz = 1, temperature_scale = 0.001, cpu_avg_load_limit = 100, @@ -27,10 +28,19 @@ llp = { --trigger assert_load_high_fault_timeout_sec = 5, clear_load_high_fault_timeout_sec = 5, - nodes = {}, + nodes = {{name="llp_cpu_mem_monitor"}, + {name="llp_disk_monitor"}, + {name="llp_gnc"}, + {name="llp_i2c"}, + {name="llp_imu"}, + {name="llp_imu_aug"}, + {name="llp_lights"}, + {name="llp_pmc"}, + {name="llp_serial"}}, } mlp = { + update_pid_hz = 0.1, update_freq_hz = 1, temperature_scale = 1.0, cpu_avg_load_limit = 100, @@ -40,5 +50,22 @@ mlp = { --trigger assert_load_high_fault_timeout_sec = 10, clear_load_high_fault_timeout_sec = 10, - nodes = {}, + nodes = {{name="mlp_arm"}, + {name="mlp_communications"}, + {name="mlp_cpu_mem_monitor"}, + {name="mlp_depth_cam"}, + {name="mlp_disk_monitor"}, + {name="mlp_dock"}, + {name="mlp_graph_localization"}, + {name="mlp_localization"}, + {name="mlp_management"}, + {name="mlp_mapper"}, + {name="mlp_mobility"}, + {name="mlp_monitors"}, + {name="mlp_multibridge"}, + {name="mlp_perch"}, + {name="mlp_recording"}, + {name="mlp_serial"}, + {name="mlp_states"}, + {name="mlp_vision"}}, } diff --git a/astrobee/config/management/data_bagger.config b/astrobee/config/management/data_bagger.config index e8d52e25b3..bed1f15835 100644 --- a/astrobee/config/management/data_bagger.config +++ b/astrobee/config/management/data_bagger.config @@ -26,7 +26,7 @@ startup_time_secs = 20 bags_save_directory = "/data/bags/" -bag_size_bytes = 96*1024*1024 +bag_size_bytes = 5*96*1024*1024 default_topics = {{topic="gnc/ctl/traj", downlink="immediate", frequency=-1}, {topic="gnc/ekf", downlink="immediate", frequency=-1}, diff --git a/astrobee/config/robots/bsharp.config b/astrobee/config/robots/bsharp.config index 21663eac5b..191cfbd2bb 100644 --- a/astrobee/config/robots/bsharp.config +++ b/astrobee/config/robots/bsharp.config @@ -24,8 +24,13 @@ robot_imu_drdy_pin = 4 robot_geometry = { -- Placeholder transforms, not accurate! - hazcam_to_navcam_transform = transform(vec3(0.079356943, 0.00019413532, -0.0019157775), quat4(0.076822296, -0.009733734, 0.99698419, 0.0051110388)), - scicam_to_hazcam_transform = transform(vec3(0.0070938664, 0.061024816, -0.11184277), quat4(0.065639737, -0.031237584, 0.99720414, -0.017307023)), + nav_cam_to_haz_cam_transform = transform(vec3(0.071421481, -0.00030319673, 0.0018058249), quat4(-0.0030431141, 0.0092646368, 0.99993195, 0.0064039206)), + nav_cam_to_sci_cam_transform = transform(vec3(-0.076526224, 0.011869553, 0.045409155), quat4(-0.0035414316, 0.0048089362, -0.0071515076, 0.99995659)), + haz_cam_depth_to_image_transform = { + 0.91602851, -0.00031586647, -0.0028485861, 0.0029767338, + 0.00032189197, 0.91603089, 0.0019373744, -0.0020741879, + 0.0028479115, -0.0019383659, 0.91602652, -0.0042296964, + 0, 0, 0, 1}, -- Calibrated transforms -- nav_cam_transform = transform(vec3(0.1157+0.002, -0.0422, -0.0826), quat4(-0.46938154, -0.52978318, -0.5317378, -0.46504373)), @@ -93,7 +98,9 @@ robot_camera_calibrations = { }, gain=50, exposure=150 - } + }, + nav_cam_to_haz_cam_timestamp_offset = -0.02, + nav_cam_to_sci_cam_timestamp_offset = 0.18 } -- PMC bus ordering and i2c trims {stbd, port} diff --git a/astrobee/config/robots/bsharp2.config b/astrobee/config/robots/bsharp2.config index ed4d527a14..2d04ee189b 100644 --- a/astrobee/config/robots/bsharp2.config +++ b/astrobee/config/robots/bsharp2.config @@ -34,14 +34,12 @@ robot_geometry = { -- dock_cam_transform = transform(vec3(-0.1032-0.0029, -0.0540, -0.0064), quat4(0.54029868, -0.45297017, -0.47464308, 0.52688644)), -- imu_transform = transform(vec3(0.0247, 0.0183, 0.0094), quat4(0.0043601096, -0.0066617904, 0.75077957, 0.66050535)), - hazcam_to_navcam_transform = transform(vec3(0.078032110432164, 0.0042875352601025, 0.014128477959323), quat4(-0.0027776443, -0.018759555, 0.9998178, -0.0021748829)), - scicam_to_hazcam_transform = transform(vec3(0.034551960811004, 0.022188043489199, -0.013680922388742), quat4(0.0017295218, -0.028156155, 0.99960204, 7.9377659e-06)), - navcam_to_hazcam_timestamp_offset = -0.02, - scicam_to_hazcam_timestamp_offset = -0.21, - hazcam_depth_to_image_transform = { - 0.94687664237117, -0.00042839578619649, -0.0009798676950181, 0.001373455007722, - 0.00042815681004901, 0.94687712204638, -0.00023113888106316, 0.00046083490888393, - 0.0009799721467424, 0.00023069565137983, 0.9468767108962, 0.00056469038710861, + nav_cam_to_haz_cam_transform = transform(vec3(0.079350378, 0.0020932693, -0.025811147), quat4(0.0045827434, -0.00067115432, 0.99998583, 0.0026256072)), + nav_cam_to_sci_cam_transform = transform(vec3(-0.041219515, 0.019857798, 0.015677018), quat4(0.0019155044, 0.00092629811, -0.0019321095, 0.99999587)), + haz_cam_depth_to_image_transform = { + 0.94713911, 0.00063710851, 0.0043044568, 0.0084247659, + -0.00057512557, 0.94311399, 0.032836596, 0.00075736623, + -0.016608396, -0.036455481, 0.95435862, -0.026870438, 0, 0, 0, 1}, -- Engineering positions with idealized orientations @@ -56,10 +54,10 @@ robot_geometry = { robot_camera_calibrations = { nav_cam = { - distortion_coeff = 1.0033897, + distortion_coeff = 1.0092039, intrinsic_matrix = { - 620.67509, 0.0, 580.25394, - 0.0, 620.67509, 494.45803, + 621.04422, 0.0, 580.56427, + 0.0, 621.04422, 495.51236, 0.0, 0.0, 1.0 }, gain=100, @@ -86,25 +84,27 @@ nav_cam = { exposure=150 }, haz_cam = { - distortion_coeff = {-0.36056524, 0.063303105, 0.0017514406, 3.3229736e-05}, + distortion_coeff = {-0.34478639, 0.029176425, 0.0061109035, 0.0011360463}, intrinsic_matrix = { - 219.1313, 0.0, 110.9028, - 0.0, 219.1313, 90.52451, + 215.92492, 0.0, 108.00909, + 0.0, 215.92492, 82.795963, 0.0, 0.0, 1.0 }, gain=100, exposure=150 }, sci_cam = { - distortion_coeff = {0.00013126118, 0.011836873, 0.0014470664, -0.00011076796}, + distortion_coeff = {-0.011275619, 0.049569258, -0.00085412642, 0.0016215327}, intrinsic_matrix = { - 1040.0823, 0.0, 663.45304, - 0.0, 1040.0823, 523.50871, + 1056.7224, 0.0, 668.60496, + 0.0, 1056.7224, 507.31654, 0.0, 0.0, 1.0 }, gain=100, exposure=150 }, + nav_cam_to_haz_cam_timestamp_offset = -0.02, + nav_cam_to_sci_cam_timestamp_offset = -0.01 } -- PMC bus ordering and i2c trims {stbd, port} diff --git a/astrobee/config/robots/bumble.config b/astrobee/config/robots/bumble.config index c1f73031e8..6d88929620 100644 --- a/astrobee/config/robots/bumble.config +++ b/astrobee/config/robots/bumble.config @@ -15,7 +15,7 @@ -- License for the specific language governing permissions and limitations -- under the License. --- bumblebee config +-- bumble config robot_llp_address = "10.42.0.8" robot_mlp_address = "10.42.0.9" @@ -25,14 +25,12 @@ robot_i2c_bus = "/dev/i2c-1" robot_imu_drdy_pin = 4 robot_geometry = { - hazcam_to_navcam_transform = transform(vec3(0.07142937, 0.00058221217, -0.001373669), quat4(-0.0030431141, 0.0092646368, 0.99993195, -0.0064039206)), - scicam_to_hazcam_transform = transform(vec3(-0.0052887445, 0.010298013, -0.043606689), quat4(0.0018545621, 0.012796392, 0.99991616, -0.00069204825)), - navcam_to_hazcam_timestamp_offset = -0.02, - scicam_to_hazcam_timestamp_offset = 0.5, - hazcam_depth_to_image_transform = { - 0.91602851, -0.00031586647, -0.0028485861, 0.0029767338, - 0.00032189197, 0.91603089, 0.0019373744, -0.0020741879, - 0.0028479115, -0.0019383659, 0.91602652, -0.0042296964, + nav_cam_to_haz_cam_transform = transform(vec3(0.072789474, 0.0010512418, -0.048805003), quat4(-0.00014444836, 0.020741299, 0.99976044, 0.006988339)), + nav_cam_to_sci_cam_transform = transform(vec3(-0.035350038, 0.019131216, 0.0083921009), quat4(0.0007718104, -0.0043221056, -0.0069533111, 0.99996619)), + haz_cam_depth_to_image_transform = { + 0.91037857, 8.4134405e-05, 0.0050848035, 0.0020508297, + -0.0003594422, 0.9113776, 0.023508033, -0.0011003268, + -0.0044235394, -0.018620972, 0.88773717, -0.028333293, 0, 0, 0, 1}, -- Engineering positions with idealized orientations @@ -42,7 +40,7 @@ robot_geometry = { dock_cam_transform = transform(vec3(-0.1032-0.0029, -0.0540, -0.0064), quat4(0.500, -0.500, -0.500, 0.500) ), --imu_transform = transform(vec3(0.0247, 0.0183, 0.0094), quat4(0.000, 0.000, 0.70710678118, 0.70710678118) ), imu_transform = transform(vec3(0.0386, 0.0247, -0.01016), quat4(0.000, 0.000, 0.70710678118, 0.70710678118) ), - -- Not accurate only for sim purposes + -- Not accurate, only for sim purposes sci_cam_transform = transform(vec3(0.118, 0.0, -0.096), quat4(0.500, 0.500, 0.500, 0.500) ) }; @@ -78,25 +76,27 @@ robot_camera_calibrations = { exposure=150 }, haz_cam = { - distortion_coeff = {-0.050689743, -1.1461691, -0.001373226, -0.00056427513}, + distortion_coeff = {-0.259498, -0.08484934, 0.0032980311, -0.00024045673}, intrinsic_matrix = { - 215.88697, 0.0, 114.12908, - 0.0, 215.88697, 86.547331, + 206.19095, 0.0, 112.48999, + 0.0, 206.19095, 81.216598, 0.0, 0.0, 1.0 }, gain=50, exposure=150 }, sci_cam = { - distortion_coeff = {0.029948958, -0.056854916, -0.00077885482, -0.0039909885}, + distortion_coeff = {-0.025598438, 0.048258987, -0.00041380657, 0.0056673533}, intrinsic_matrix = { - 1048.499, 0.0, 705.22914, - 0.0, 1048.499, 511.03079, + 1023.6054, 0.0, 683.97547, + 0.0, 1023.6054, 511.2185, 0.0, 0.0, 1.0 }, gain=50, exposure=150 - } + }, + nav_cam_to_haz_cam_timestamp_offset = -0.02, + nav_cam_to_sci_cam_timestamp_offset = 0.08 } -- PMC bus ordering and i2c trims {stbd, port} diff --git a/astrobee/config/robots/flatsat.config b/astrobee/config/robots/flatsat.config index 6ca9b5b433..6be2ca7292 100644 --- a/astrobee/config/robots/flatsat.config +++ b/astrobee/config/robots/flatsat.config @@ -24,8 +24,13 @@ robot_imu_drdy_pin = 4 robot_geometry = { -- Placeholder transforms, not accurate! - hazcam_to_navcam_transform = transform(vec3(0.079356943, 0.00019413532, -0.0019157775), quat4(0.076822296, -0.009733734, 0.99698419, 0.0051110388)), - scicam_to_hazcam_transform = transform(vec3(0.0070938664, 0.061024816, -0.11184277), quat4(0.065639737, -0.031237584, 0.99720414, -0.017307023)), + nav_cam_to_haz_cam_transform = transform(vec3(0.071421481, -0.00030319673, 0.0018058249), quat4(-0.0030431141, 0.0092646368, 0.99993195, 0.0064039206)), + nav_cam_to_sci_cam_transform = transform(vec3(-0.076526224, 0.011869553, 0.045409155), quat4(-0.0035414316, 0.0048089362, -0.0071515076, 0.99995659)), + haz_cam_depth_to_image_transform = { + 0.91602851, -0.00031586647, -0.0028485861, 0.0029767338, + 0.00032189197, 0.91603089, 0.0019373744, -0.0020741879, + 0.0028479115, -0.0019383659, 0.91602652, -0.0042296964, + 0, 0, 0, 1}, perch_cam_transform = transform(vec3(-0.1375, 0.055, -0.03), quat4(0, -0.70711, 0, 0.70711)), haz_cam_transform = transform(vec3(0, 0, 0), quat4(0, 0, 0, 1)), -- placeholder, not valid! @@ -96,7 +101,9 @@ robot_camera_calibrations = { }, gain=50, exposure=150 - } + }, + nav_cam_to_haz_cam_timestamp_offset = -0.02, + nav_cam_to_sci_cam_timestamp_offset = 0.18 } -- robot_perch_cam_device = "0004-3608-0024-1107" diff --git a/astrobee/config/robots/honey.config b/astrobee/config/robots/honey.config index 50fa9cb504..fc8b814061 100644 --- a/astrobee/config/robots/honey.config +++ b/astrobee/config/robots/honey.config @@ -26,8 +26,13 @@ robot_imu_drdy_pin = 4 robot_geometry = { -- Placeholder transforms, not accurate! - hazcam_to_navcam_transform = transform(vec3(0.079356943, 0.00019413532, -0.0019157775), quat4(0.076822296, -0.009733734, 0.99698419, 0.0051110388)), - scicam_to_hazcam_transform = transform(vec3(0.0070938664, 0.061024816, -0.11184277), quat4(0.065639737, -0.031237584, 0.99720414, -0.017307023)), + nav_cam_to_haz_cam_transform = transform(vec3(0.071421481, -0.00030319673, 0.0018058249), quat4(-0.0030431141, 0.0092646368, 0.99993195, 0.0064039206)), + nav_cam_to_sci_cam_transform = transform(vec3(-0.076526224, 0.011869553, 0.045409155), quat4(-0.0035414316, 0.0048089362, -0.0071515076, 0.99995659)), + haz_cam_depth_to_image_transform = { + 0.91602851, -0.00031586647, -0.0028485861, 0.0029767338, + 0.00032189197, 0.91603089, 0.0019373744, -0.0020741879, + 0.0028479115, -0.0019383659, 0.91602652, -0.0042296964, + 0, 0, 0, 1}, -- Engineering positions with idealized orientations perch_cam_transform = transform(vec3(-0.1331, 0.0509, -0.0166), quat4(0.000, -0.70710678118, 0.000, 0.70710678118)),-- placeholder, not valid! @@ -92,7 +97,9 @@ robot_camera_calibrations = { }, gain=50, exposure=150 - } + }, + nav_cam_to_haz_cam_timestamp_offset = -0.02, + nav_cam_to_sci_cam_timestamp_offset = 0.18 } -- PMC bus ordering and i2c trims {stbd, port} diff --git a/astrobee/config/robots/killer.config b/astrobee/config/robots/killer.config index 0284d6aa01..b983162f8a 100644 --- a/astrobee/config/robots/killer.config +++ b/astrobee/config/robots/killer.config @@ -24,8 +24,13 @@ robot_imu_drdy_pin = 4 robot_geometry = { -- Placeholder transforms, not accurate! - hazcam_to_navcam_transform = transform(vec3(0.079356943, 0.00019413532, -0.0019157775), quat4(0.076822296, -0.009733734, 0.99698419, 0.0051110388)), - scicam_to_hazcam_transform = transform(vec3(0.0070938664, 0.061024816, -0.11184277), quat4(0.065639737, -0.031237584, 0.99720414, -0.017307023)), + nav_cam_to_haz_cam_transform = transform(vec3(0.071421481, -0.00030319673, 0.0018058249), quat4(-0.0030431141, 0.0092646368, 0.99993195, 0.0064039206)), + nav_cam_to_sci_cam_transform = transform(vec3(-0.076526224, 0.011869553, 0.045409155), quat4(-0.0035414316, 0.0048089362, -0.0071515076, 0.99995659)), + haz_cam_depth_to_image_transform = { + 0.91602851, -0.00031586647, -0.0028485861, 0.0029767338, + 0.00032189197, 0.91603089, 0.0019373744, -0.0020741879, + 0.0028479115, -0.0019383659, 0.91602652, -0.0042296964, + 0, 0, 0, 1}, perch_cam_transform = transform(vec3(-0.1375, 0.055, -0.03), quat4(0, -0.70711, 0, 0.70711)), haz_cam_transform = transform(vec3(0, 0, 0), quat4(0, 0, 0, 1)), -- placeholder, not valid! @@ -96,7 +101,9 @@ robot_camera_calibrations = { }, gain=50, exposure=150 - } + }, + nav_cam_to_haz_cam_timestamp_offset = -0.02, + nav_cam_to_sci_cam_timestamp_offset = 0.18 } robot_perch_cam_device = "0005-4805-0050-1520" diff --git a/astrobee/config/robots/melissa.config b/astrobee/config/robots/melissa.config index 139e6f5edc..04d4dd5ce1 100644 --- a/astrobee/config/robots/melissa.config +++ b/astrobee/config/robots/melissa.config @@ -26,8 +26,13 @@ robot_imu_drdy_pin = 4 robot_geometry = { -- Placeholder transforms, not accurate! - hazcam_to_navcam_transform = transform(vec3(0.079356943, 0.00019413532, -0.0019157775), quat4(0.076822296, -0.009733734, 0.99698419, 0.0051110388)), - scicam_to_hazcam_transform = transform(vec3(0.0070938664, 0.061024816, -0.11184277), quat4(0.065639737, -0.031237584, 0.99720414, -0.017307023)), + nav_cam_to_haz_cam_transform = transform(vec3(0.071421481, -0.00030319673, 0.0018058249), quat4(-0.0030431141, 0.0092646368, 0.99993195, 0.0064039206)), + nav_cam_to_sci_cam_transform = transform(vec3(-0.076526224, 0.011869553, 0.045409155), quat4(-0.0035414316, 0.0048089362, -0.0071515076, 0.99995659)), + haz_cam_depth_to_image_transform = { + 0.91602851, -0.00031586647, -0.0028485861, 0.0029767338, + 0.00032189197, 0.91603089, 0.0019373744, -0.0020741879, + 0.0028479115, -0.0019383659, 0.91602652, -0.0042296964, + 0, 0, 0, 1}, -- Engineering positions with idealized orientations perch_cam_transform = transform(vec3(-0.1331, 0.0509, -0.0166), quat4(0.000, -0.70710678118, 0.000, 0.70710678118)),-- placeholder, not valid! @@ -90,7 +95,9 @@ robot_camera_calibrations = { }, gain=50, exposure=150 - } + }, + nav_cam_to_haz_cam_timestamp_offset = -0.02, + nav_cam_to_sci_cam_timestamp_offset = 0.18 } -- PMC bus ordering and i2c trims {stbd, port} diff --git a/astrobee/config/robots/p4c.config b/astrobee/config/robots/p4c.config index c94d56757e..ba6adeb600 100644 --- a/astrobee/config/robots/p4c.config +++ b/astrobee/config/robots/p4c.config @@ -24,8 +24,13 @@ robot_imu_drdy_pin = 101 robot_geometry = { -- Placeholder transforms, not accurate! - hazcam_to_navcam_transform = transform(vec3(0.079356943, 0.00019413532, -0.0019157775), quat4(0.076822296, -0.009733734, 0.99698419, 0.0051110388)), - scicam_to_hazcam_transform = transform(vec3(0.0070938664, 0.061024816, -0.11184277), quat4(0.065639737, -0.031237584, 0.99720414, -0.017307023)), + nav_cam_to_haz_cam_transform = transform(vec3(0.071421481, -0.00030319673, 0.0018058249), quat4(-0.0030431141, 0.0092646368, 0.99993195, 0.0064039206)), + nav_cam_to_sci_cam_transform = transform(vec3(-0.076526224, 0.011869553, 0.045409155), quat4(-0.0035414316, 0.0048089362, -0.0071515076, 0.99995659)), + haz_cam_depth_to_image_transform = { + 0.91602851, -0.00031586647, -0.0028485861, 0.0029767338, + 0.00032189197, 0.91603089, 0.0019373744, -0.0020741879, + 0.0028479115, -0.0019383659, 0.91602652, -0.0042296964, + 0, 0, 0, 1}, perch_cam_transform = transform(vec3(-0.1375, 0.055, -0.03), quat4(0, -0.70711, 0, 0.70711)), haz_cam_transform = transform(vec3(0, 0, 0), quat4(0, 0, 0, 1)), -- placeholder, not valid! @@ -92,7 +97,9 @@ robot_camera_calibrations = { }, gain=50, exposure=150 - } + }, + nav_cam_to_haz_cam_timestamp_offset = -0.02, + nav_cam_to_sci_cam_timestamp_offset = 0.18 } -- PMC bus ordering and i2c trims {stbd, port} diff --git a/astrobee/config/robots/p4d.config b/astrobee/config/robots/p4d.config index d70a64669d..127d96dc9d 100644 --- a/astrobee/config/robots/p4d.config +++ b/astrobee/config/robots/p4d.config @@ -24,8 +24,13 @@ robot_imu_drdy_pin = 101 robot_geometry = { -- Placeholder transforms, not accurate! - hazcam_to_navcam_transform = transform(vec3(0.079356943, 0.00019413532, -0.0019157775), quat4(0.076822296, -0.009733734, 0.99698419, 0.0051110388)), - scicam_to_hazcam_transform = transform(vec3(0.0070938664, 0.061024816, -0.11184277), quat4(0.065639737, -0.031237584, 0.99720414, -0.017307023)), + nav_cam_to_haz_cam_transform = transform(vec3(0.071421481, -0.00030319673, 0.0018058249), quat4(-0.0030431141, 0.0092646368, 0.99993195, 0.0064039206)), + nav_cam_to_sci_cam_transform = transform(vec3(-0.076526224, 0.011869553, 0.045409155), quat4(-0.0035414316, 0.0048089362, -0.0071515076, 0.99995659)), + haz_cam_depth_to_image_transform = { + 0.91602851, -0.00031586647, -0.0028485861, 0.0029767338, + 0.00032189197, 0.91603089, 0.0019373744, -0.0020741879, + 0.0028479115, -0.0019383659, 0.91602652, -0.0042296964, + 0, 0, 0, 1}, perch_cam_transform = transform(vec3(-0.17520466, 0.040140345, -0.049281679), quat4(0.027207383, -0.71130843, 0.013260087, 0.70222806)), haz_cam_transform = transform(vec3(0.092136082, 0.041911327, -0.11237338), quat4(-0.51485943, 0.49197658, -0.47825571, 0.51395553)), -- placeholder, not valid! @@ -94,7 +99,9 @@ robot_camera_calibrations = { }, gain=50, exposure=150 - } + }, + nav_cam_to_haz_cam_timestamp_offset = -0.02, + nav_cam_to_sci_cam_timestamp_offset = 0.18 } -- PMC bus ordering and i2c trims {stbd, port} diff --git a/astrobee/config/robots/queen.config b/astrobee/config/robots/queen.config index fe5953c911..1ce9fe35ef 100644 --- a/astrobee/config/robots/queen.config +++ b/astrobee/config/robots/queen.config @@ -26,8 +26,13 @@ robot_imu_drdy_pin = 4 robot_geometry = { -- Placeholder transforms, not accurate! - hazcam_to_navcam_transform = transform(vec3(0.079356943, 0.00019413532, -0.0019157775), quat4(0.076822296, -0.009733734, 0.99698419, 0.0051110388)), - scicam_to_hazcam_transform = transform(vec3(0.0070938664, 0.061024816, -0.11184277), quat4(0.065639737, -0.031237584, 0.99720414, -0.017307023)), + nav_cam_to_haz_cam_transform = transform(vec3(0.071421481, -0.00030319673, 0.0018058249), quat4(-0.0030431141, 0.0092646368, 0.99993195, 0.0064039206)), + nav_cam_to_sci_cam_transform = transform(vec3(-0.076526224, 0.011869553, 0.045409155), quat4(-0.0035414316, 0.0048089362, -0.0071515076, 0.99995659)), + haz_cam_depth_to_image_transform = { + 0.91602851, -0.00031586647, -0.0028485861, 0.0029767338, + 0.00032189197, 0.91603089, 0.0019373744, -0.0020741879, + 0.0028479115, -0.0019383659, 0.91602652, -0.0042296964, + 0, 0, 0, 1}, -- Engineering positions with idealized orientations perch_cam_transform = transform(vec3(-0.1331, 0.0509, -0.0166), quat4(0.000, -0.70710678118, 0.000, 0.70710678118)),-- placeholder, not valid! @@ -91,7 +96,9 @@ robot_camera_calibrations = { }, gain=50, exposure=150 - } + }, + nav_cam_to_haz_cam_timestamp_offset = -0.02, + nav_cam_to_sci_cam_timestamp_offset = 0.18 } -- PMC bus ordering and i2c trims {stbd, port} diff --git a/astrobee/config/robots/sim.config b/astrobee/config/robots/sim.config index 58f4c80869..2510117a97 100644 --- a/astrobee/config/robots/sim.config +++ b/astrobee/config/robots/sim.config @@ -25,8 +25,13 @@ robot_imu_drdy_pin = 4 -- Engineering positions and idealized orientations robot_geometry = { -- Placeholder transforms, not accurate! - hazcam_to_navcam_transform = transform(vec3(0.079356943, 0.00019413532, -0.0019157775), quat4(0.076822296, -0.009733734, 0.99698419, 0.0051110388)), - scicam_to_hazcam_transform = transform(vec3(0.0070938664, 0.061024816, -0.11184277), quat4(0.065639737, -0.031237584, 0.99720414, -0.017307023)), + nav_cam_to_haz_cam_transform = transform(vec3(0.071421481, -0.00030319673, 0.0018058249), quat4(-0.0030431141, 0.0092646368, 0.99993195, 0.0064039206)), + nav_cam_to_sci_cam_transform = transform(vec3(-0.076526224, 0.011869553, 0.045409155), quat4(-0.0035414316, 0.0048089362, -0.0071515076, 0.99995659)), + haz_cam_depth_to_image_transform = { + 0.91602851, -0.00031586647, -0.0028485861, 0.0029767338, + 0.00032189197, 0.91603089, 0.0019373744, -0.0020741879, + 0.0028479115, -0.0019383659, 0.91602652, -0.0042296964, + 0, 0, 0, 1}, perch_cam_transform = transform(vec3(-0.1331, 0.0509, -0.0166), quat4(0.000, -0.70710678118, 0.000, 0.70710678118)),-- placeholder, not valid! haz_cam_transform = transform(vec3(0.1328, 0.0362, -0.0826), quat4(-0.500, 0.500, -0.500, 0.500)), -- placeholder, not valid! @@ -89,7 +94,9 @@ robot_camera_calibrations = { }, gain=50, exposure=150 - } + }, + nav_cam_to_haz_cam_timestamp_offset = -0.02, + nav_cam_to_sci_cam_timestamp_offset = 0.18 } robot_perch_cam_device = "" diff --git a/astrobee/config/robots/wannabee.config b/astrobee/config/robots/wannabee.config index 937dec68f5..c78ebfcc4e 100644 --- a/astrobee/config/robots/wannabee.config +++ b/astrobee/config/robots/wannabee.config @@ -24,8 +24,13 @@ robot_imu_drdy_pin = 4 robot_geometry = { -- Placeholder transforms, not accurate! - hazcam_to_navcam_transform = transform(vec3(0.079356943, 0.00019413532, -0.0019157775), quat4(0.076822296, -0.009733734, 0.99698419, 0.0051110388)), - scicam_to_hazcam_transform = transform(vec3(0.0070938664, 0.061024816, -0.11184277), quat4(0.065639737, -0.031237584, 0.99720414, -0.017307023)), + nav_cam_to_haz_cam_transform = transform(vec3(0.071421481, -0.00030319673, 0.0018058249), quat4(-0.0030431141, 0.0092646368, 0.99993195, 0.0064039206)), + nav_cam_to_sci_cam_transform = transform(vec3(-0.076526224, 0.011869553, 0.045409155), quat4(-0.0035414316, 0.0048089362, -0.0071515076, 0.99995659)), + haz_cam_depth_to_image_transform = { + 0.91602851, -0.00031586647, -0.0028485861, 0.0029767338, + 0.00032189197, 0.91603089, 0.0019373744, -0.0020741879, + 0.0028479115, -0.0019383659, 0.91602652, -0.0042296964, + 0, 0, 0, 1}, -- Calibrated transforms -- nav_cam_transform = transform(vec3(0.1157+0.002, -0.0422, -0.0826), quat4(-0.46938154, -0.52978318, -0.5317378, -0.46504373)), @@ -93,7 +98,9 @@ robot_camera_calibrations = { }, gain=50, exposure=150 - } + }, + nav_cam_to_haz_cam_timestamp_offset = -0.02, + nav_cam_to_sci_cam_timestamp_offset = 0.18 } -- PMC bus ordering and i2c trims {stbd, port} diff --git a/astrobee/config/tools/graph_bag.config b/astrobee/config/tools/graph_bag.config index 55969624f5..3008537967 100644 --- a/astrobee/config/tools/graph_bag.config +++ b/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 diff --git a/astrobee/config/transforms.config b/astrobee/config/transforms.config index 92642622e0..01750605d4 100644 --- a/astrobee/config/transforms.config +++ b/astrobee/config/transforms.config @@ -41,13 +41,6 @@ transforms = { { global = false, parent = "body", child = "sci_cam", transform = sci_cam_transform }, - -- TRANSFORMS BETWEEN CAMERAS - -- For some reason this interfers with placing the bot in the right location - -- { global = false, parent = "haz_cam", child = "nav_cam", - -- transform = hazcam_to_navcam_transform }, - -- { global = false, parent = "sci_cam", child = "haz_cam", - -- transform = scicam_to_hazcam_transform }, - -- IMU { global = false, parent = "body", child = "imu", diff --git a/astrobee/launch/offline_localization/enable_localization b/astrobee/launch/offline_localization/enable_localization new file mode 100755 index 0000000000..5fe9311f96 --- /dev/null +++ b/astrobee/launch/offline_localization/enable_localization @@ -0,0 +1,15 @@ +#!/bin/bash + +# Run the command to enable localization. If it fails, try again after a short time. + +while [ 1 ]; do + rosservice call --wait /loc/ml/enable true + ans=$? + if [ "$ans" -eq 0 ]; then + break + fi + sleep 0.25 +done + + + diff --git a/astrobee/launch/offline_localization/loc_rviz.launch b/astrobee/launch/offline_localization/loc_rviz.launch index 58001c1cf0..b0317858a9 100644 --- a/astrobee/launch/offline_localization/loc_rviz.launch +++ b/astrobee/launch/offline_localization/loc_rviz.launch @@ -31,8 +31,6 @@ - - diff --git a/astrobee/launch/offline_localization/localization_from_bag.launch b/astrobee/launch/offline_localization/localization_from_bag.launch index 20305126c0..54fbd81976 100644 --- a/astrobee/launch/offline_localization/localization_from_bag.launch +++ b/astrobee/launch/offline_localization/localization_from_bag.launch @@ -24,13 +24,14 @@ - + + - + @@ -79,6 +80,13 @@ + + + + + + + diff --git a/astrobee/launch/offline_localization/record_localization.launch b/astrobee/launch/offline_localization/record_localization.launch index 2df4d712bb..a5ab1c75d8 100644 --- a/astrobee/launch/offline_localization/record_localization.launch +++ b/astrobee/launch/offline_localization/record_localization.launch @@ -18,5 +18,5 @@ - + diff --git a/astrobee/launch/offline_localization/replay_localization.launch b/astrobee/launch/offline_localization/replay_localization.launch index f5756ebcae..7f822936c9 100644 --- a/astrobee/launch/offline_localization/replay_localization.launch +++ b/astrobee/launch/offline_localization/replay_localization.launch @@ -29,5 +29,5 @@ - + diff --git a/astrobee/launch/offline_localization/sparse_mapping_matching_from_bag.launch b/astrobee/launch/offline_localization/sparse_mapping_matching_from_bag.launch new file mode 100644 index 0000000000..b4b9d841f3 --- /dev/null +++ b/astrobee/launch/offline_localization/sparse_mapping_matching_from_bag.launch @@ -0,0 +1,67 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/astrobee/launch/robot/MLP.launch b/astrobee/launch/robot/MLP.launch index 1381bea27a..b2a8fe6e7c 100644 --- a/astrobee/launch/robot/MLP.launch +++ b/astrobee/launch/robot/MLP.launch @@ -38,7 +38,7 @@ + name="mlp_mapper" output="$(arg output)" launch-prefix="nice -n 10"/> + + + + + + + + + + @@ -387,7 +397,16 @@ - + + + + + + + + + + diff --git a/astrobee/launch/sim.launch b/astrobee/launch/sim.launch index d390178151..6f27924d7f 100644 --- a/astrobee/launch/sim.launch +++ b/astrobee/launch/sim.launch @@ -65,13 +65,16 @@ name="pose" default="0 0 -0.7 0 0 0 1" /> - - + + + + + @@ -168,7 +171,10 @@ - + + @@ -209,7 +215,10 @@ - + + diff --git a/astrobee/readme.md b/astrobee/readme.md index 55df00836b..aac20de4ba 100644 --- a/astrobee/readme.md +++ b/astrobee/readme.md @@ -131,6 +131,9 @@ connection to the LLP (in case it is absent from the test rig): `roslaunch astrobee spawn.launch world:=iss ns:=bumble` +See more details in \ref running-the-sim for how to run the robot in +simulation. + 4. Do a processor-in-the-loop simulation `roslaunch astrobee sim.launch llp:=10.42.0.10 mlp:=10.42.0.11` diff --git a/astrobee/resources/rviz/iss.rviz b/astrobee/resources/rviz/iss.rviz index 9b3b76074f..60279c7f72 100644 --- a/astrobee/resources/rviz/iss.rviz +++ b/astrobee/resources/rviz/iss.rviz @@ -531,7 +531,7 @@ Visualization Manager: Name: "DEBUG: SciCam" Normalize Range: true Queue Size: 2 - Transport Hint: raw + Transport Hint: compressed Unreliable: false Value: false - Class: rviz/Image diff --git a/astrobee/resources/rviz/iss_viz.rviz b/astrobee/resources/rviz/iss_viz.rviz index e1481638f6..b35796e36c 100644 --- a/astrobee/resources/rviz/iss_viz.rviz +++ b/astrobee/resources/rviz/iss_viz.rviz @@ -475,7 +475,7 @@ Visualization Manager: Name: "DEBUG: SciCam" Normalize Range: true Queue Size: 2 - Transport Hint: raw + Transport Hint: compressed Unreliable: false Value: false - Class: rviz/Image diff --git a/communications/dds_msgs/CMakeLists.txt b/communications/dds_msgs/CMakeLists.txt index c593054aa4..025c41089d 100644 --- a/communications/dds_msgs/CMakeLists.txt +++ b/communications/dds_msgs/CMakeLists.txt @@ -35,7 +35,17 @@ if (USE_DDS) set(MIRO_ROOT_DIR ${SORACORE_ROOT_DIR}) list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/../../cmake") - find_package(Qt4 4.6.0 REQUIRED QtXml) + + # find Qt version according to OS + find_program(LSB_RELEASE_EXEC lsb_release) + execute_process(COMMAND "${LSB_RELEASE_EXEC}" --short --release OUTPUT_VARIABLE LSB_RELEASE_VERSION_SHORT OUTPUT_STRIP_TRAILING_WHITESPACE) + + if (${LSB_RELEASE_VERSION_SHORT} VERSION_GREATER 18.04) + find_package(Qt5Xml REQUIRED) + else () + find_package(Qt4 4.6.0 REQUIRED QtXml) + endif () + find_package(Miro REQUIRED) find_package(RtiDds REQUIRED) diff --git a/communications/dds_ros_bridge/CMakeLists.txt b/communications/dds_ros_bridge/CMakeLists.txt index c0ee7aa106..b86f728931 100644 --- a/communications/dds_ros_bridge/CMakeLists.txt +++ b/communications/dds_ros_bridge/CMakeLists.txt @@ -49,8 +49,16 @@ set(MIRO_ROOT_DIR ${SORACORE_ROOT_DIR}) SET(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) SET(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) -# just calls the normal one -find_package(Qt4 4.6.0 REQUIRED QtXml) +# find Qt version according to OS +find_program(LSB_RELEASE_EXEC lsb_release) +execute_process(COMMAND "${LSB_RELEASE_EXEC}" --short --release OUTPUT_VARIABLE LSB_RELEASE_VERSION_SHORT OUTPUT_STRIP_TRAILING_WHITESPACE) + +if (${LSB_RELEASE_VERSION_SHORT} VERSION_GREATER 18.04) + find_package(Qt5Xml REQUIRED) +else () + find_package(Qt4 4.6.0 REQUIRED QtXml) +endif () + find_package(Miro REQUIRED) find_package(RtiDds REQUIRED) find_package(Soracore REQUIRED) @@ -88,15 +96,22 @@ add_library(dds_ros_bridge ) target_compile_definitions(dds_ros_bridge PUBLIC ${RTIDDS_DEFINE_FLAGS}) add_dependencies(dds_ros_bridge ${catkin_EXPORTED_TARGETS}) -target_link_libraries(dds_ros_bridge rapidIo ${Boost_LIBRARIES} ${catkin_LIBRARIES}) - +if (${LSB_RELEASE_VERSION_SHORT} VERSION_GREATER 18.04) + target_link_libraries(dds_ros_bridge rapidIo Qt5::Xml ${Boost_LIBRARIES} ${catkin_LIBRARIES}) +else () + target_link_libraries(dds_ros_bridge rapidIo ${Boost_LIBRARIES} ${catkin_LIBRARIES}) +endif () add_library(astrobee_astrobee_bridge src/astrobee_astrobee_bridge.cc ) target_compile_definitions(astrobee_astrobee_bridge PUBLIC ${RTIDDS_DEFINE_FLAGS}) add_dependencies(astrobee_astrobee_bridge ${catkin_EXPORTED_TARGETS}) -target_link_libraries(astrobee_astrobee_bridge Qt4::QtXml rapidIo ${Boost_LIBRARIES} ${catkin_LIBRARIES}) +if (${LSB_RELEASE_VERSION_SHORT} VERSION_GREATER 18.04) + target_link_libraries(astrobee_astrobee_bridge Qt5::Xml rapidIo ${Boost_LIBRARIES} ${catkin_LIBRARIES}) +else () + target_link_libraries(astrobee_astrobee_bridge rapidIo ${Boost_LIBRARIES} ${catkin_LIBRARIES}) +endif () ############# diff --git a/communications/ff_msgs/msg/CpuNodeState.msg b/communications/ff_msgs/msg/CpuNodeState.msg new file mode 100644 index 0000000000..f9e9ad345c --- /dev/null +++ b/communications/ff_msgs/msg/CpuNodeState.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. +# +# State of a CPU load for a node. + +# Node name +string name + +# The load (in percentages) of the cpu +float32 load diff --git a/communications/ff_msgs/msg/CpuStateStamped.msg b/communications/ff_msgs/msg/CpuStateStamped.msg index 4df093f118..2a6e2402f5 100644 --- a/communications/ff_msgs/msg/CpuStateStamped.msg +++ b/communications/ff_msgs/msg/CpuStateStamped.msg @@ -45,3 +45,5 @@ float32 temp # or not all of them are enabled. ff_msgs/CpuState[] cpus +# Load usage of individual ROS nodes +ff_msgs/CpuNodeState[] load_nodes \ No newline at end of file diff --git a/communications/ff_msgs/msg/DepthCorrespondence.msg b/communications/ff_msgs/msg/DepthCorrespondence.msg new file mode 100644 index 0000000000..2ef1b765ec --- /dev/null +++ b/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 diff --git a/communications/ff_msgs/msg/DepthOdometry.msg b/communications/ff_msgs/msg/DepthOdometry.msg new file mode 100644 index 0000000000..d6461babfa --- /dev/null +++ b/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 diff --git a/communications/ff_msgs/msg/ImagePoint.msg b/communications/ff_msgs/msg/ImagePoint.msg new file mode 100644 index 0000000000..7dddac685a --- /dev/null +++ b/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 diff --git a/communications/ff_msgs/msg/Odometry.msg b/communications/ff_msgs/msg/Odometry.msg new file mode 100644 index 0000000000..c8d7c86fb0 --- /dev/null +++ b/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 diff --git a/debian/changelog b/debian/changelog index 5cd459ed68..3c09cf9b41 100644 --- a/debian/changelog +++ b/debian/changelog @@ -1,3 +1,11 @@ +astrobee (0.16.1) testing; urgency=medium + + * depth odometry + * better performance evaluation + * multiple other fixes and improvements + + -- Astrobee Flight Software Wed, 12 Jan 2022 14:55:11 -0800 + astrobee (0.16.0) testing; urgency=medium * project compiles with catkin diff --git a/doc/general_documentation/INSTALL.md b/doc/general_documentation/INSTALL.md index ef25d418c3..215ef7ad4b 100644 --- a/doc/general_documentation/INSTALL.md +++ b/doc/general_documentation/INSTALL.md @@ -102,7 +102,14 @@ of turning on and off options. To see which options are supported, simply run source ~/.bashrc popd -The configure script modifies your ``.bashrc`` to source ``setup.bash`` for +In you run a Zsh session, then + + pushd $ASTROBEE_WS + ./src/scripts/configure.sh -l -F -D + source ~/.zshrc + popd + +The configure script modifies your ``.bashrc``/``.zshrc`` to source ``setup.bash``/``setup.zsh`` for the current ROS distribution and to set CMAKE_PREFIX_PATH. It is suggested to examine it and see if all changes were made correctly. diff --git a/doc/general_documentation/astrobee_usage.md b/doc/general_documentation/astrobee_usage.md index 618adaf4ca..3fe5b8ba53 100644 --- a/doc/general_documentation/astrobee_usage.md +++ b/doc/general_documentation/astrobee_usage.md @@ -3,6 +3,8 @@ \subpage astrobee +\subpage astrobee-code-style + \subpage teleop \subpage release diff --git a/hardware/pico_driver/CMakeLists.txt b/hardware/pico_driver/CMakeLists.txt index 18bdfc62cd..2d1a7215f3 100644 --- a/hardware/pico_driver/CMakeLists.txt +++ b/hardware/pico_driver/CMakeLists.txt @@ -59,8 +59,9 @@ add_library(pico_driver add_dependencies(pico_driver ${catkin_EXPORTED_TARGETS}) target_link_libraries(pico_driver ${royale_LIBRARIES} ${EIGEN_LIBRARIES} ${catkin_LIBRARIES}) -## Declare a C++ executable: bag_to_csv -add_executable(pico_proxy tools/pico_proxy.cc) +add_library(pico_proxy + src/pico_proxy.cc +) add_dependencies(pico_proxy ${catkin_EXPORTED_TARGETS}) target_link_libraries(pico_proxy pico_driver gflags glog ${catkin_LIBRARIES}) @@ -94,6 +95,11 @@ install(TARGETS ${PROJECT_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} ) +install(TARGETS pico_proxy + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) # Mark nodelet_plugin for installation install(FILES nodelet_plugins.xml @@ -101,10 +107,8 @@ install(FILES nodelet_plugins.xml ) # Install C++ executables -install(TARGETS pico_proxy DESTINATION bin) install(TARGETS pico_tool DESTINATION bin) install(CODE "execute_process( - COMMAND ln -s ../../bin/pico_proxy share/${PROJECT_NAME} COMMAND ln -s ../../bin/pico_tool share/${PROJECT_NAME} WORKING_DIRECTORY ${CMAKE_INSTALL_PREFIX} OUTPUT_QUIET diff --git a/hardware/pico_driver/launch/pico_proxy.launch b/hardware/pico_driver/launch/pico_proxy.launch index 1a06461447..45f7766df0 100644 --- a/hardware/pico_driver/launch/pico_proxy.launch +++ b/hardware/pico_driver/launch/pico_proxy.launch @@ -15,12 +15,13 @@ - - - - - - - - + + + + + + + + + diff --git a/hardware/pico_driver/nodelet_plugins.xml b/hardware/pico_driver/nodelet_plugins.xml index 2ac4fa2928..9c65bb3490 100644 --- a/hardware/pico_driver/nodelet_plugins.xml +++ b/hardware/pico_driver/nodelet_plugins.xml @@ -1,5 +1,12 @@ - - - Nodelet for pico_driver::PicoDriverNodelet - - \ No newline at end of file + + + + Nodelet for pico_driver::PicoDriverNodelet + + + + + Nodelet for pico_proxy::PicoProxyNodelet + + + \ No newline at end of file diff --git a/hardware/pico_driver/src/pico_proxy.cc b/hardware/pico_driver/src/pico_proxy.cc new file mode 100644 index 0000000000..291a5e6302 --- /dev/null +++ b/hardware/pico_driver/src/pico_proxy.cc @@ -0,0 +1,246 @@ +/* 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. + */ + +// Core ROS +#include +#include +#include + +// Shared libraries +#include +#include +#include + +#include +#include +#include +#include +#include + +// Messages +#include +#include + +#include +#include + +/** + * \ingroup hardware + */ +namespace pico_proxy { + +class PicoProxyNodelet : public ff_util::FreeFlyerNodelet { + public: + PicoProxyNodelet() : ff_util::FreeFlyerNodelet(false) {} + ~PicoProxyNodelet() {} + + protected: + void Initialize(ros::NodeHandle *nh) { + std::string topic, topic_type; + topic = TOPIC_HARDWARE_PICOFLEXX_PREFIX; + config_name_ = GetName(); + // Read the camera name + if (config_name_.find("haz") != std::string::npos) { + topic += TOPIC_HARDWARE_NAME_HAZ_CAM; + } else if (config_name_.find("perch") != std::string::npos) { + topic += TOPIC_HARDWARE_NAME_PERCH_CAM; + } else { + NODELET_FATAL("You need to pass a camera name to the pico proxy nodelet name"); + } + + // Read the topic type + if (config_name_.find("extended") != std::string::npos) { + topic += TOPIC_HARDWARE_PICOFLEXX_SUFFIX_EXTENDED; + + // Read config file for amplitude factor + config_.AddFile("cameras.config"); + if (!ReadParams()) + return; + + pub_d_ = nh->advertise(topic + "/distance/", 1); + pub_a_ = nh->advertise(topic + "/amplitude/", 1); + pub_i_ = nh->advertise(topic + "/intensity/", 1); + pub_c_ = nh->advertise(topic + "/noise/", 1); + pub_a_int_ = nh->advertise(topic + "/amplitude_int/", 1); + sub_ = nh->subscribe(topic, 1, &PicoProxyNodelet::ExtendedCallback, this); + + } else if (config_name_.find("depth_image") != std::string::npos) { + topic += TOPIC_HARDWARE_PICOFLEXX_SUFFIX_DEPTH_IMAGE; + + pub_d_ = nh->advertise(topic + "/distance", 1); + pub_c_ = nh->advertise(topic + "/confidence", 1); + sub_ = nh->subscribe(topic, 1, &PicoProxyNodelet::DepthImageCallback, this); + + } else { + NODELET_FATAL("The pico proxy nodelet name must have extended or depth_image"); + } + } + + bool ReadParams(void) { + // Read the config file + if (!config_.ReadFiles()) { + NODELET_FATAL("Failed to read config files."); + return false; + } + // Try and open the config file + config_reader::ConfigReader::Table pconfig; + if (!config_.GetTable("picoflexx", &pconfig)) { + NODELET_FATAL("Picoflexx config not found in LUA."); + return false; + } + // Read the device information from the config table + config_reader::ConfigReader::Table devices; + if (!pconfig.GetTable("devices", &devices)) { + NODELET_FATAL("Lua:Could get devices item in config file"); + } + + for (int i = 0; i < devices.GetSize(); i++) { + // Get the device info + config_reader::ConfigReader::Table device_info; + if (!devices.GetTable(i + 1, &device_info)) { + NODELET_FATAL("Lua:Could get row in table table"); + continue; + } + + // Get the parameters + std::string name; + if (!device_info.GetStr("name", &name)) { + NODELET_FATAL("Lua:Could not find row 'name' in table"); + continue; + } + + if (config_name_.find(name) == std::string::npos) + continue; + + // Query the amplitude factor + if (device_info.GetReal("amplitude_factor", &litude_factor_)) + return true; + } + NODELET_FATAL("You need to pass the amplitude factor to camera parameters"); + return false; + } + + struct null_deleter { + void operator()(void const *) const {} + }; + + // Called when extended data arrives + void ExtendedCallback(const ff_msgs::PicoflexxIntermediateData::ConstPtr& msg) { + cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare( + sensor_msgs::ImageConstPtr(&msg->raw, null_deleter()), + sensor_msgs::image_encodings::TYPE_32FC4); + cv::split(cv_ptr->image, layers_); + + // Keep the same timestamp as the original data + std_msgs::Header header; + header.stamp = msg->header.stamp; + header.frame_id = msg->header.frame_id; + + cv_bridge::CvImage d(header, sensor_msgs::image_encodings::TYPE_32FC1, layers_[0]); + cv_bridge::CvImage a(header, sensor_msgs::image_encodings::TYPE_32FC1, layers_[1]); + cv_bridge::CvImage i(header, sensor_msgs::image_encodings::TYPE_32FC1, layers_[2]); + cv_bridge::CvImage n(header, sensor_msgs::image_encodings::TYPE_32FC1, layers_[3]); + + // Kalibr cannot handle float images. Hence, we need to create an + // integer version of the amplitude topic (while keeping the + // original, as it is used in other contexts). + + // TODO(oalexan1): I found out that kalibr cannot handle a float + // amplitude. Hence, here it is multiplied by amplitude_factor and + // cast to uint16. The best experimental value for amplitude_factor + // turned out to be 100, but this may need further + // experimentation. Casting to uint8 was not enough. This will need + // a deeper study. If the amplitude image looks too saturated or too + // dark, a different amplitude factor can be set when launching this. + layers_[1].convertTo(img_int_, CV_16UC1, amplitude_factor_, 0); + cv_bridge::CvImage a_int(header, sensor_msgs::image_encodings::MONO16, img_int_); + + // Publish individual images + pub_d_.publish(d.toImageMsg()); + pub_a_.publish(a.toImageMsg()); + pub_i_.publish(i.toImageMsg()); + pub_c_.publish(n.toImageMsg()); + + pub_a_int_.publish(a_int.toImageMsg()); + } + + // Called when depth image data arrives + void DepthImageCallback(const sensor_msgs::ImageConstPtr& msg) { + // Prepare distance image + // TODO(oalexan1): keep same timestamp as the input? + distance_.header.stamp = ros::Time::now(); + distance_.header.frame_id = msg->header.frame_id; + distance_.height = msg->height; + distance_.width = msg->width; + distance_.is_bigendian = msg->is_bigendian; + distance_.encoding = sensor_msgs::image_encodings::TYPE_16UC1; + distance_.step = distance_.width * sizeof(uint16_t); + distance_.data.resize(distance_.height * distance_.step); + // Prepare confidence image + // TODO(oalexan1): keep same timestamp as the input? + confidence_.header.stamp = ros::Time::now(); + confidence_.header.frame_id = msg->header.frame_id; + confidence_.height = msg->height; + confidence_.width = msg->width; + confidence_.is_bigendian = msg->is_bigendian; + confidence_.encoding = sensor_msgs::image_encodings::TYPE_8UC1; + confidence_.step = confidence_.width * sizeof(uint8_t); + confidence_.data.resize(confidence_.height * confidence_.step); + // Set pointers to the data fields of the corresponding ROS images + uint16_t *msg_ptr = const_cast( + reinterpret_cast(&msg->data[0])); + uint16_t *dist_ptr = reinterpret_cast(&distance_.data[0]); + uint8_t *conf_ptr = reinterpret_cast(&confidence_.data[0]); + // Iterate through the image found in msg + for (size_t i = 0; i < (distance_.width * distance_.height); + ++i, ++msg_ptr, ++dist_ptr, ++conf_ptr) { + *dist_ptr = *msg_ptr & 0x1fff; // Grab the distance + *conf_ptr = (uint8_t)((*msg_ptr & 0xE000) >> 13); // Grab the confidence + } + pub_d_.publish(distance_); + pub_c_.publish(confidence_); + } + + private: + // Configs + config_reader::ConfigReader config_; + std::string config_name_; + + // Publishers + ros::Publisher pub_d_; + ros::Publisher pub_a_; + ros::Publisher pub_i_; + ros::Publisher pub_c_; + // Subscriber + ros::Subscriber sub_; + + ros::Publisher pub_a_int_; + + // Layer extraction + std::vector layers_ = std::vector(4); + cv::Mat img_int_; + + // Depth and confidence matrices + sensor_msgs::Image distance_, confidence_; + + double amplitude_factor_; +}; + +PLUGINLIB_EXPORT_CLASS(pico_proxy::PicoProxyNodelet, nodelet::Nodelet); + +} // namespace pico_proxy diff --git a/hardware/pico_driver/tools/pico_proxy.cc b/hardware/pico_driver/tools/pico_proxy.cc deleted file mode 100644 index 9fa1c0a6c1..0000000000 --- a/hardware/pico_driver/tools/pico_proxy.cc +++ /dev/null @@ -1,163 +0,0 @@ -/* 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. - */ - -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include - -// Publishers -ros::Publisher pub_d_; -ros::Publisher pub_a_; -ros::Publisher pub_i_; -ros::Publisher pub_c_; - -ros::Publisher pub_a_int_; - -// Layer extraction -std::vector layers(4); -cv::Mat img_int; - -// Depth and confidence matrices -sensor_msgs::Image distance_, confidence_; - -double amplitude_factor; - -struct null_deleter { - void operator()(void const *) const {} -}; - -// Called when extended data arrives -void ExtendedCallback(const ff_msgs::PicoflexxIntermediateData::ConstPtr& msg) { - cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare( - sensor_msgs::ImageConstPtr(&msg->raw, null_deleter()), - sensor_msgs::image_encodings::TYPE_32FC4); - cv::split(cv_ptr->image, layers); - - // Keep the same timestamp as the original data - std_msgs::Header header; - header.stamp = msg->header.stamp; - header.frame_id = msg->header.frame_id; - - cv_bridge::CvImage d(header, sensor_msgs::image_encodings::TYPE_32FC1, layers[0]); - cv_bridge::CvImage a(header, sensor_msgs::image_encodings::TYPE_32FC1, layers[1]); - cv_bridge::CvImage i(header, sensor_msgs::image_encodings::TYPE_32FC1, layers[2]); - cv_bridge::CvImage n(header, sensor_msgs::image_encodings::TYPE_32FC1, layers[3]); - - // Kalibr cannot handle float images. Hence, we need to create an - // integer version of the amplitude topic (while keeping the - // original, as it is used in other contexts). - - // TODO(oalexan1): I found out that kalibr cannot handle a float - // amplitude. Hence, here it is multiplied by amplitude_factor and - // cast to uint16. The best experimental value for amplitude_factor - // turned out to be 100, but this may need further - // experimentation. Casting to uint8 was not enough. This will need - // a deeper study. If the amplitude image looks too saturated or too - // dark, a different amplitude factor can be set when launching this. - layers[1].convertTo(img_int, CV_16UC1, amplitude_factor, 0); - cv_bridge::CvImage a_int(header, sensor_msgs::image_encodings::MONO16, img_int); - - // Publish individual images - pub_d_.publish(d.toImageMsg()); - pub_a_.publish(a.toImageMsg()); - pub_i_.publish(i.toImageMsg()); - pub_c_.publish(n.toImageMsg()); - - pub_a_int_.publish(a_int.toImageMsg()); -} - -// Called when depth image data arrives -void DepthImageCallback(const sensor_msgs::ImageConstPtr& msg) { - // Prepare distance image - // TODO(oalexan1): keep same timestamp as the input? - distance_.header.stamp = ros::Time::now(); - distance_.header.frame_id = msg->header.frame_id; - distance_.height = msg->height; - distance_.width = msg->width; - distance_.is_bigendian = msg->is_bigendian; - distance_.encoding = sensor_msgs::image_encodings::TYPE_16UC1; - distance_.step = distance_.width * sizeof(uint16_t); - distance_.data.resize(distance_.height * distance_.step); - // Prepare confidence image - // TODO(oalexan1): keep same timestamp as the input? - confidence_.header.stamp = ros::Time::now(); - confidence_.header.frame_id = msg->header.frame_id; - confidence_.height = msg->height; - confidence_.width = msg->width; - confidence_.is_bigendian = msg->is_bigendian; - confidence_.encoding = sensor_msgs::image_encodings::TYPE_8UC1; - confidence_.step = confidence_.width * sizeof(uint8_t); - confidence_.data.resize(confidence_.height * confidence_.step); - // Set pointers to the data fields of the corresponding ROS images - uint16_t *msg_ptr = const_cast( - reinterpret_cast(&msg->data[0])); - uint16_t *dist_ptr = reinterpret_cast(&distance_.data[0]); - uint8_t *conf_ptr = reinterpret_cast(&confidence_.data[0]); - // Iterate through the image found in msg - for (size_t i = 0; i < (distance_.width * distance_.height); - ++i, ++msg_ptr, ++dist_ptr, ++conf_ptr) { - *dist_ptr = *msg_ptr & 0x1fff; // Grab the distance - *conf_ptr = (uint8_t)((*msg_ptr & 0xE000) >> 13); // Grab the confidence - } - pub_d_.publish(distance_); - pub_c_.publish(confidence_); -} - -// Main entry point of application -int main(int argc, char **argv) { - ros::init(argc, argv, "listener"); - ros::NodeHandle n("~"); - if (!n.hasParam("topic")) - ROS_FATAL("You need to pass a topic to the pico proxy"); - if (!n.hasParam("topic_type")) - ROS_FATAL("You need to pass a topic type to the pico proxy"); - if (!n.hasParam("amplitude_factor")) - ROS_FATAL("You need to pass the amplitude factor to the pico proxy"); - std::string topic, topic_type; - n.getParam("topic", topic); - n.getParam("topic_type", topic_type); - n.getParam("amplitude_factor", amplitude_factor); - ROS_INFO_STREAM("Listening on topic " << topic); - ROS_INFO_STREAM("Using amplitude factor " << amplitude_factor); - ros::Subscriber sub; - if (topic_type == "extended") { - pub_d_ = n.advertise(topic + "/distance/", 1); - pub_a_ = n.advertise(topic + "/amplitude/", 1); - pub_i_ = n.advertise(topic + "/intensity/", 1); - pub_c_ = n.advertise(topic + "/noise/", 1); - pub_a_int_ = n.advertise(topic + "/amplitude_int/", 1); - sub = n.subscribe(topic, 1, ExtendedCallback); - } else if (topic_type == "depth_image") { - pub_d_ = n.advertise(topic + "/distance", 1); - pub_c_ = n.advertise(topic + "/confidence", 1); - sub = n.subscribe(topic, 1, DepthImageCallback); - } else { - ROS_FATAL("Unsupported type (must be \"extended\" or \"depth_image\")"); - } - ros::spin(); - return 0; -} diff --git a/hardware/smart_dock/CMakeLists.txt b/hardware/smart_dock/CMakeLists.txt index 33ab1059e3..9a3429897d 100644 --- a/hardware/smart_dock/CMakeLists.txt +++ b/hardware/smart_dock/CMakeLists.txt @@ -47,8 +47,16 @@ if (USE_DDS) SET(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) SET(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) - # just calls the normal one - find_package(Qt4 4.6.0 REQUIRED QtXml) + # find Qt version according to OS + find_program(LSB_RELEASE_EXEC lsb_release) + execute_process(COMMAND "${LSB_RELEASE_EXEC}" --short --release OUTPUT_VARIABLE LSB_RELEASE_VERSION_SHORT OUTPUT_STRIP_TRAILING_WHITESPACE) + + if (${LSB_RELEASE_VERSION_SHORT} VERSION_GREATER 18.04) + find_package(Qt5Xml REQUIRED) + else () + find_package(Qt4 4.6.0 REQUIRED QtXml) + endif () + find_package(Miro REQUIRED) find_package(RtiDds REQUIRED) find_package(Soracore REQUIRED) @@ -86,8 +94,11 @@ if (USE_DDS) ) target_compile_definitions(smart_dock PUBLIC ${RTIDDS_DEFINE_FLAGS}) add_dependencies(smart_dock ${catkin_EXPORTED_TARGETS}) - target_link_libraries(smart_dock rapidIo ${catkin_LIBRARIES}) - + if (${LSB_RELEASE_VERSION_SHORT} VERSION_GREATER 18.04) + target_link_libraries(smart_dock rapidIo Qt5::Xml ${catkin_LIBRARIES}) + else () + target_link_libraries(smart_dock rapidIo ${catkin_LIBRARIES}) + endif () ## Declare a C++ executable: smart_dock_service add_executable(smart_dock_service tools/smart_dock_service.cc) add_dependencies(smart_dock_service ${catkin_EXPORTED_TARGETS}) @@ -128,4 +139,4 @@ if (USE_DDS) else (USE_DDS) find_package(catkin REQUIRED COMPONENTS) catkin_package() -endif (USE_DDS) \ No newline at end of file +endif (USE_DDS) diff --git a/localization/CMakeLists.txt b/localization/CMakeLists.txt index 936c385cda..329b9d92c1 100644 --- a/localization/CMakeLists.txt +++ b/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) diff --git a/localization/camera/src/camera_params.cc b/localization/camera/src/camera_params.cc index bcf1ed30a0..0a3baa8a80 100644 --- a/localization/camera/src/camera_params.cc +++ b/localization/camera/src/camera_params.cc @@ -122,16 +122,21 @@ camera::CameraParameters::CameraParameters(config_reader::ConfigReader* config, optical_offset_ << cam_mat.at(0, 2), cam_mat.at(1, 2); focal_length_ << cam_mat.at(0, 0), cam_mat.at(1, 1); - // Read in the distortion coefficients (if not a real number, may be a list) + // Read in the distortion coefficients. It can be a number or a list. Eigen::VectorXd buffer(1); - if (!camera.GetReal("distortion_coeff", &buffer[0])) { - config_reader::ConfigReader::Table dist(&camera, "distortion_coeff"); + std::string dist_name = "distortion_coeff"; + if (camera.IsNumber(dist_name.c_str())) { + if (!camera.GetReal(dist_name.c_str(), &buffer[0])) + fprintf(stderr, "Could not read camera distortion_coeff."); + } else { + config_reader::ConfigReader::Table dist(&camera, dist_name.c_str()); buffer.resize(dist.GetSize()); for (int i = 0; i < dist.GetSize(); i++) { if (!dist.GetReal(i + 1, &buffer[i])) fprintf(stderr, "Could not read camera distortion_coeff."); } } + SetDistortion(buffer); if (!camera.GetInt("undistorted_width", &size[0])) diff --git a/localization/camera/tools/undistort_image.cc b/localization/camera/tools/undistort_image.cc index 1101bb0fc7..46b253c7ce 100644 --- a/localization/camera/tools/undistort_image.cc +++ b/localization/camera/tools/undistort_image.cc @@ -128,17 +128,51 @@ int main(int argc, char ** argv) { cv::Mat floating_remap, fixed_map, interp_map; cam_params.GenerateRemapMaps(&floating_remap, FLAGS_scale); - // Deal with the fact that this map may request pixels from the - // image that are out of bounds. Hence we will need to grow the - // image before interpolating into it. + // We have to conform to the OpenCV API, which says: + // undist_image(x, y) = dist_image(floating_remap(x, y)). + + // If floating_remap(x, y) is out of dist_image bounds, the above + // should return a black pixel, yet a straightforward application of + // this formula will result in a segfault. + + // The solution is to grow dist_image by padding it with black pixels + // so that the above API succeeds. + + // This can have the following problem though. floating_remap(x, y) + // can be huge for unreasonable distortion. So tame it. We only + // want to know that if this falls outside the image bounds, a black + // pixel is assigned to undist_image(x, y), so if it falls too + // much outside the image just assign it to a closer pixel outside + // the image. + float max_extra = 100.0f; // the furthest floating_remap(x, y) can deviate + + // The image dimensions + Eigen::Vector2i dims(round(FLAGS_scale*cam_params.GetDistortedSize()[0]), + round(FLAGS_scale*cam_params.GetDistortedSize()[1])); + int img_cols = dims[0], img_rows = dims[1]; - // Find the expanded image bounds cv::Vec2f start = floating_remap.at(0, 0); - double min_x = start[0], max_x = start[0]; - double min_y = start[1], max_y = start[1]; + double min_x = 0.0, max_x = 0.0, min_y = 0.0, max_y = 0.0; // will change very soon for (int col = 0; col < floating_remap.cols; col++) { for (int row = 0; row < floating_remap.rows; row++) { cv::Vec2f pix = floating_remap.at(row, col); + + // Tame floating_remap + pix[0] = std::max(pix[0], -max_extra); + pix[0] = std::min(pix[0], img_cols + max_extra); + pix[1] = std::max(pix[1], -max_extra); + pix[1] = std::min(pix[1], img_rows + max_extra); + floating_remap.at(row, col) = pix; + + if (col == 0 && row == 0) { + // initialize with the potentially adjusted value of pix. + min_x = pix[0]; + max_x = pix[0]; + min_y = pix[1]; + max_y = pix[1]; + } + + // Find the expanded (but tamed) image bounds if (pix[0] < min_x) min_x = pix[0]; if (pix[0] > max_x) @@ -149,14 +183,11 @@ int main(int argc, char ** argv) { max_y = pix[1]; } } + + // Convert the bounds to int min_x = floor(min_x); max_x = ceil(max_x); min_y = floor(min_y); max_y = ceil(max_y); - // The image dimensions - Eigen::Vector2i dims(round(FLAGS_scale*cam_params.GetDistortedSize()[0]), - round(FLAGS_scale*cam_params.GetDistortedSize()[1])); - int img_cols = dims[0], img_rows = dims[1]; - // Ensure that the expanded image is not smaller than the old one, // to make the logic simpler if (min_x > 0) @@ -210,19 +241,23 @@ int main(int argc, char ** argv) { if (undist_size[0] % 2 != 0 || undist_size[1] % 2 != 0 ) LOG(FATAL) << "The undistorted image dimensions must be even."; - int startx = (undist_size[0] - widx)/2; - int starty = (undist_size[1] - widy)/2; - - cropROI = cv::Rect(startx, starty, widx, widy); - if (cropROI.empty()) - LOG(FATAL) << "Empty crop region."; - std::cout << "Crop region: " << cropROI << std::endl; + // Ensure that the crop window is within the image bounds + int startx = std::max((undist_size[0] - widx)/2, 0); + int starty = std::max((undist_size[1] - widy)/2, 0); + widx = std::min(widx, undist_size[0] - startx); + widy = std::min(widy, undist_size[1] - starty); // Update these quantities undist_size[0] = widx; undist_size[1] = widy; optical_center[0] -= startx; optical_center[1] -= starty; + + cropROI = cv::Rect(startx, starty, widx, widy); + if (cropROI.empty()) + LOG(FATAL) << "Empty crop region."; + + std::cout << "Undistorted crop region: " << cropROI << std::endl; } for (size_t i = 0; i < images.size(); i++) { diff --git a/localization/depth_odometry/CMakeLists.txt b/localization/depth_odometry/CMakeLists.txt new file mode 100644 index 0000000000..86bb95d659 --- /dev/null +++ b/localization/depth_odometry/CMakeLists.txt @@ -0,0 +1,138 @@ +#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. + +cmake_minimum_required(VERSION 3.0) +project(depth_odometry) + +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + camera + ff_util + localization_common + localization_measurements + msg_conversions + optimization_common + point_cloud_common + vision_common +) + +# Find OpenCV3 +SET(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/../../cmake") +find_package(OpenCV331 REQUIRED) + +find_package(Eigen3 REQUIRED) + +find_package(Ceres REQUIRED) + +find_package(PCL REQUIRED COMPONENTS common search) + +find_package(pcl_conversions REQUIRED) + +catkin_package( + INCLUDE_DIRS include ${CERES_INCLUDE_DIRS} ${GTSAM_INCLUDE_DIR} ${PCL_INCLUDE_DIRS} ${pcl_conversions_INCLUDE_DIRS} + LIBRARIES ${PROJECT_NAME} ${CERES_LIBRARIES} ${PCL_LIBRARIES} ${GTSAM_LIBRARIES} + CATKIN_DEPENDS roscpp ff_msgs ff_util localization_common localization_measurements msg_conversions optimization_common point_cloud_common vision_common + DEPENDS gtsam pcl +) + +########### +## Build ## +########### +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${CERES_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} + ${pcl_conversions_INCLUDE_DIRS} +) + +# Declare C++ libraries +add_library(${PROJECT_NAME} + src/depth_odometry_nodelet.cc + src/depth_odometry_wrapper.cc + src/image_features_with_known_correspondences_aligner_depth_odometry.cc + src/parameter_reader.cc + src/point_to_plane_icp_depth_odometry.cc + src/utilities.cc +) +add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES} gtsam) + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest_gtest(test_point_to_plane_icp_depth_odometry + test/test_point_to_plane_icp_depth_odometry.test + test/test_point_to_plane_icp_depth_odometry.cc + test/test_utilities.cc + ) + target_link_libraries(test_point_to_plane_icp_depth_odometry + ${PROJECT_NAME} ${catkin_LIBRARIES} + ) + add_rostest_gtest(test_image_features_with_known_correspondences_aligner_depth_odometry + test/test_image_features_with_known_correspondences_aligner_depth_odometry.test + test/test_image_features_with_known_correspondences_aligner_depth_odometry.cc + test/test_utilities.cc + ) + target_link_libraries(test_image_features_with_known_correspondences_aligner_depth_odometry + ${PROJECT_NAME} ${catkin_LIBRARIES} + ) + + find_package(rostest REQUIRED) + add_rostest_gtest(test_depth_odometry_wrapper + test/test_depth_odometry_wrapper.test + test/test_depth_odometry_wrapper.cc + test/test_utilities.cc + ) + target_link_libraries(test_depth_odometry_wrapper + ${PROJECT_NAME} ${catkin_LIBRARIES} + ) + +endif() + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark nodelet_plugin for installation +install(FILES nodelet_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +# Mark launch files for installation +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + PATTERN ".svn" EXCLUDE) diff --git a/localization/depth_odometry/include/depth_odometry/depth_image_features_and_points.h b/localization/depth_odometry/include/depth_odometry/depth_image_features_and_points.h new file mode 100644 index 0000000000..d8996fe5f2 --- /dev/null +++ b/localization/depth_odometry/include/depth_odometry/depth_image_features_and_points.h @@ -0,0 +1,72 @@ +/* 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. + */ + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#ifndef DEPTH_ODOMETRY_DEPTH_IMAGE_FEATURES_AND_POINTS_H_ +#define DEPTH_ODOMETRY_DEPTH_IMAGE_FEATURES_AND_POINTS_H_ + +namespace depth_odometry { +class DepthImageFeaturesAndPoints { + public: + // TODO(rsoussan): Pass clahe as boost optional ref! add default as boost none! + DepthImageFeaturesAndPoints(const localization_measurements::DepthImage& depth_image, cv::Feature2D& feature_detector, + const cv::Ptr clahe, const bool normals_required = false) + : depth_image_(depth_image) { + if (clahe) { + cv::Mat clahe_image; + clahe->apply(depth_image_.image(), clahe_image); + feature_image_.reset(new vision_common::FeatureImage(clahe_image, feature_detector)); + } else { + feature_image_.reset(new vision_common::FeatureImage(depth_image_.image(), feature_detector)); + } + + if (normals_required) { + kdtree_.reset(new pcl::search::KdTree()); + filtered_point_cloud_ = + point_cloud_common::FilteredPointCloud(depth_image_.unfiltered_point_cloud()); + kdtree_->setInputCloud(filtered_point_cloud_); + } + } + + boost::optional Normal(const Eigen::Vector3d& point_3d, const double search_radius) const { + return point_cloud_common::GetNormal(point_3d, *filtered_point_cloud_, *kdtree_, search_radius); + } + + const vision_common::FeatureImage& feature_image() const { return *feature_image_; } + + const localization_measurements::DepthImage& depth_image() const { return depth_image_; } + + private: + localization_measurements::DepthImage depth_image_; + std::unique_ptr feature_image_; + pcl::search::KdTree::Ptr kdtree_; + pcl::PointCloud::Ptr filtered_point_cloud_; +}; +} // namespace depth_odometry +#endif // DEPTH_ODOMETRY_DEPTH_IMAGE_FEATURES_AND_POINTS_H_ diff --git a/localization/depth_odometry/include/depth_odometry/depth_odometry.h b/localization/depth_odometry/include/depth_odometry/depth_odometry.h new file mode 100644 index 0000000000..11ac64b7b6 --- /dev/null +++ b/localization/depth_odometry/include/depth_odometry/depth_odometry.h @@ -0,0 +1,36 @@ +/* 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. + */ +#ifndef DEPTH_ODOMETRY_DEPTH_ODOMETRY_H_ +#define DEPTH_ODOMETRY_DEPTH_ODOMETRY_H_ + +#include +#include +#include +#include + +#include + +namespace depth_odometry { +class DepthOdometry { + public: + virtual boost::optional DepthImageCallback( + const localization_measurements::DepthImageMeasurement& depth_image) = 0; +}; +} // namespace depth_odometry + +#endif // DEPTH_ODOMETRY_DEPTH_ODOMETRY_H_ diff --git a/localization/depth_odometry/include/depth_odometry/depth_odometry_nodelet.h b/localization/depth_odometry/include/depth_odometry/depth_odometry_nodelet.h new file mode 100644 index 0000000000..02a7e5f1d9 --- /dev/null +++ b/localization/depth_odometry/include/depth_odometry/depth_odometry_nodelet.h @@ -0,0 +1,53 @@ +/* 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. + */ +#ifndef DEPTH_ODOMETRY_DEPTH_ODOMETRY_NODELET_H_ +#define DEPTH_ODOMETRY_DEPTH_ODOMETRY_NODELET_H_ + +#include +#include +#include + +#include + +#include +#include +#include +#include + +namespace depth_odometry { +class DepthOdometryNodelet : public ff_util::FreeFlyerNodelet { + public: + DepthOdometryNodelet(); + + private: + void Initialize(ros::NodeHandle* nh) final; + void SubscribeAndAdvertise(ros::NodeHandle* nh); + void PointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& point_cloud_msg); + void ImageCallback(const sensor_msgs::ImageConstPtr& image_msg); + bool EnableService(ff_msgs::SetBool::Request& req, ff_msgs::SetBool::Response& res); + + DepthOdometryWrapper depth_odometry_wrapper_; + image_transport::Subscriber image_sub_; + ros::Subscriber point_cloud_sub_; + ros::Publisher depth_odometry_pub_; + ros::ServiceServer enable_srv_; + bool enabled_; +}; +} // namespace depth_odometry + +#endif // DEPTH_ODOMETRY_DEPTH_ODOMETRY_NODELET_H_ diff --git a/localization/depth_odometry/include/depth_odometry/depth_odometry_params.h b/localization/depth_odometry/include/depth_odometry/depth_odometry_params.h new file mode 100644 index 0000000000..6565d91094 --- /dev/null +++ b/localization/depth_odometry/include/depth_odometry/depth_odometry_params.h @@ -0,0 +1,29 @@ +/* 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. + */ +#ifndef DEPTH_ODOMETRY_DEPTH_ODOMETRY_PARAMS_H_ +#define DEPTH_ODOMETRY_DEPTH_ODOMETRY_PARAMS_H_ + +namespace depth_odometry { +struct DepthOdometryParams { + double max_time_diff; + double position_covariance_threshold; + double orientation_covariance_threshold; +}; +} // namespace depth_odometry + +#endif // DEPTH_ODOMETRY_DEPTH_ODOMETRY_PARAMS_H_ diff --git a/localization/depth_odometry/include/depth_odometry/depth_odometry_wrapper.h b/localization/depth_odometry/include/depth_odometry/depth_odometry_wrapper.h new file mode 100644 index 0000000000..d0292511a2 --- /dev/null +++ b/localization/depth_odometry/include/depth_odometry/depth_odometry_wrapper.h @@ -0,0 +1,57 @@ +/* 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. + */ +#ifndef DEPTH_ODOMETRY_DEPTH_ODOMETRY_WRAPPER_H_ +#define DEPTH_ODOMETRY_DEPTH_ODOMETRY_WRAPPER_H_ + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include + +#include +#include + +namespace depth_odometry { +class DepthOdometryWrapper { + public: + explicit DepthOdometryWrapper(const std::string& path_prefix = "localization/"); + explicit DepthOdometryWrapper(const DepthOdometryWrapperParams& params); + std::vector PointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& point_cloud_msg); + std::vector ImageCallback(const sensor_msgs::ImageConstPtr& image_msg); + + private: + void Initialize(const DepthOdometryWrapperParams& params); + std::vector ProcessDepthImageIfAvailable(); + std::unique_ptr depth_odometry_; + DepthOdometryWrapperParams params_; + localization_common::MeasurementBuffer point_cloud_buffer_; + localization_common::MeasurementBuffer image_buffer_; + localization_common::Timer timer_ = localization_common::Timer("Depth Odometry"); +}; +} // namespace depth_odometry + +#endif // DEPTH_ODOMETRY_DEPTH_ODOMETRY_WRAPPER_H_ diff --git a/localization/depth_odometry/include/depth_odometry/depth_odometry_wrapper_params.h b/localization/depth_odometry/include/depth_odometry/depth_odometry_wrapper_params.h new file mode 100644 index 0000000000..956fd4202f --- /dev/null +++ b/localization/depth_odometry/include/depth_odometry/depth_odometry_wrapper_params.h @@ -0,0 +1,40 @@ +/* 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. + */ +#ifndef DEPTH_ODOMETRY_DEPTH_ODOMETRY_WRAPPER_PARAMS_H_ +#define DEPTH_ODOMETRY_DEPTH_ODOMETRY_WRAPPER_PARAMS_H_ + +#include +#include + +#include + +#include + +namespace depth_odometry { +struct DepthOdometryWrapperParams { + double max_image_and_point_cloud_time_diff; + // icp or image_feature + std::string method; + Eigen::Isometry3d body_T_haz_cam; + Eigen::Affine3d haz_cam_A_haz_depth; + PointToPlaneICPDepthOdometryParams icp; + ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams image_features; +}; +} // namespace depth_odometry + +#endif // DEPTH_ODOMETRY_DEPTH_ODOMETRY_WRAPPER_PARAMS_H_ diff --git a/localization/depth_odometry/include/depth_odometry/image_features_with_known_correspondences_aligner_depth_odometry.h b/localization/depth_odometry/include/depth_odometry/image_features_with_known_correspondences_aligner_depth_odometry.h new file mode 100644 index 0000000000..f4e497ab5b --- /dev/null +++ b/localization/depth_odometry/include/depth_odometry/image_features_with_known_correspondences_aligner_depth_odometry.h @@ -0,0 +1,55 @@ +/* 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. + */ +#ifndef DEPTH_ODOMETRY_IMAGE_FEATURES_WITH_KNOWN_CORRESPONDENCES_ALIGNER_DEPTH_ODOMETRY_H_ +#define DEPTH_ODOMETRY_IMAGE_FEATURES_WITH_KNOWN_CORRESPONDENCES_ALIGNER_DEPTH_ODOMETRY_H_ + +#include +#include +#include +#include +#include +#include +#include + +namespace depth_odometry { +class ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry : public DepthOdometry { + public: + explicit ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry( + const ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams& params); + boost::optional DepthImageCallback( + const localization_measurements::DepthImageMeasurement& depth_image) final; + const ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams& params() const { return params_; } + + private: + bool ValidImagePoint(const Eigen::Vector2d& image_point) const; + bool Valid3dPoint(const boost::optional& point) const; + + ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams params_; + point_cloud_common::PointCloudWithKnownCorrespondencesAligner aligner_; + std::shared_ptr previous_depth_image_features_and_points_; + std::shared_ptr latest_depth_image_features_and_points_; + localization_common::Time previous_timestamp_; + localization_common::Time latest_timestamp_; + std::unique_ptr feature_detector_and_matcher_; + cv::Ptr clahe_; + bool normals_required_; + boost::optional point_to_plane_icp_depth_odometry_; +}; +} // namespace depth_odometry + +#endif // DEPTH_ODOMETRY_IMAGE_FEATURES_WITH_KNOWN_CORRESPONDENCES_ALIGNER_DEPTH_ODOMETRY_H_ diff --git a/localization/depth_odometry/include/depth_odometry/image_features_with_known_correspondences_aligner_depth_odometry_params.h b/localization/depth_odometry/include/depth_odometry/image_features_with_known_correspondences_aligner_depth_odometry_params.h new file mode 100644 index 0000000000..3416822159 --- /dev/null +++ b/localization/depth_odometry/include/depth_odometry/image_features_with_known_correspondences_aligner_depth_odometry_params.h @@ -0,0 +1,50 @@ +/* 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. + */ +#ifndef DEPTH_ODOMETRY_IMAGE_FEATURES_WITH_KNOWN_CORRESPONDENCES_ALIGNER_DEPTH_ODOMETRY_PARAMS_H_ +#define DEPTH_ODOMETRY_IMAGE_FEATURES_WITH_KNOWN_CORRESPONDENCES_ALIGNER_DEPTH_ODOMETRY_PARAMS_H_ + +#include +#include +#include +#include +#include +#include + +#include + +namespace depth_odometry { +struct ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams : public DepthOdometryParams { + point_cloud_common::PointCloudWithKnownCorrespondencesAlignerParams aligner; + vision_common::BriskFeatureDetectorAndMatcherParams brisk_feature_detector_and_matcher; + vision_common::LKOpticalFlowFeatureDetectorAndMatcherParams lk_optical_flow_feature_detector_and_matcher; + vision_common::SurfFeatureDetectorAndMatcherParams surf_feature_detector_and_matcher; + std::string detector; + // CLAHE params + bool use_clahe; + int clahe_grid_length; + double clahe_clip_limit; + // Other + double min_x_distance_to_border; + double min_y_distance_to_border; + int min_num_inliers; + bool refine_estimate; + PointToPlaneICPDepthOdometryParams point_to_plane_icp; +}; +} // namespace depth_odometry + +#endif // DEPTH_ODOMETRY_IMAGE_FEATURES_WITH_KNOWN_CORRESPONDENCES_ALIGNER_DEPTH_ODOMETRY_PARAMS_H_ diff --git a/localization/depth_odometry/include/depth_odometry/parameter_reader.h b/localization/depth_odometry/include/depth_odometry/parameter_reader.h new file mode 100644 index 0000000000..966cd0b29e --- /dev/null +++ b/localization/depth_odometry/include/depth_odometry/parameter_reader.h @@ -0,0 +1,35 @@ +/* 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. + */ +#ifndef DEPTH_ODOMETRY_PARAMETER_READER_H_ +#define DEPTH_ODOMETRY_PARAMETER_READER_H_ + +#include +#include +#include +#include +#include + +namespace depth_odometry { +void LoadDepthOdometryParams(config_reader::ConfigReader& config, DepthOdometryParams& params); +void LoadDepthOdometryWrapperParams(config_reader::ConfigReader& config, DepthOdometryWrapperParams& params); +void LoadPointToPlaneICPDepthOdometryParams(config_reader::ConfigReader& config, + PointToPlaneICPDepthOdometryParams& params); +void LoadImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams( + config_reader::ConfigReader& config, ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams& params); +} // namespace depth_odometry +#endif // DEPTH_ODOMETRY_PARAMETER_READER_H_ diff --git a/localization/depth_odometry/include/depth_odometry/point_to_plane_icp_depth_odometry.h b/localization/depth_odometry/include/depth_odometry/point_to_plane_icp_depth_odometry.h new file mode 100644 index 0000000000..a85d623826 --- /dev/null +++ b/localization/depth_odometry/include/depth_odometry/point_to_plane_icp_depth_odometry.h @@ -0,0 +1,50 @@ +/* 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. + */ +#ifndef DEPTH_ODOMETRY_POINT_TO_PLANE_ICP_DEPTH_ODOMETRY_H_ +#define DEPTH_ODOMETRY_POINT_TO_PLANE_ICP_DEPTH_ODOMETRY_H_ + +#include +#include +#include +#include + +namespace depth_odometry { +class PointToPlaneICPDepthOdometry : public DepthOdometry { + public: + explicit PointToPlaneICPDepthOdometry(const PointToPlaneICPDepthOdometryParams& params); + boost::optional DepthImageCallback( + const localization_measurements::DepthImageMeasurement& depth_image_measurement) final; + boost::optional DepthImageCallbackWithEstimate( + const localization_measurements::DepthImageMeasurement& depth_image_measurement, + const boost::optional target_T_source_initial_estimate = boost::none); + const PointToPlaneICPDepthOdometryParams& params() const { return params_; } + + private: + pcl::PointCloud::Ptr DownsampleAndFilterCloud( + const pcl::PointCloud::Ptr& cloud) const; + + PointToPlaneICPDepthOdometryParams params_; + point_cloud_common::PointToPlaneICP icp_; + pcl::PointCloud::Ptr previous_point_cloud_with_normals_; + pcl::PointCloud::Ptr latest_point_cloud_with_normals_; + localization_common::Time previous_timestamp_; + localization_common::Time latest_timestamp_; +}; +} // namespace depth_odometry + +#endif // DEPTH_ODOMETRY_POINT_TO_PLANE_ICP_DEPTH_ODOMETRY_H_ diff --git a/localization/depth_odometry/include/depth_odometry/point_to_plane_icp_depth_odometry_params.h b/localization/depth_odometry/include/depth_odometry/point_to_plane_icp_depth_odometry_params.h new file mode 100644 index 0000000000..6864813306 --- /dev/null +++ b/localization/depth_odometry/include/depth_odometry/point_to_plane_icp_depth_odometry_params.h @@ -0,0 +1,44 @@ +/* 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. + */ +#ifndef DEPTH_ODOMETRY_POINT_TO_PLANE_ICP_DEPTH_ODOMETRY_PARAMS_H_ +#define DEPTH_ODOMETRY_POINT_TO_PLANE_ICP_DEPTH_ODOMETRY_PARAMS_H_ + +#include +#include + +#include + +namespace depth_odometry { +struct PointToPlaneICPDepthOdometryParams : public DepthOdometryParams { + point_cloud_common::PointToPlaneICPParams icp; + bool downsample; + double downsample_leaf_size; + // Organized normal estimation + bool use_organized_normal_estimation; + pcl::IntegralImageNormalEstimation::NormalEstimationMethod normal_estimation_method; + bool use_depth_dependent_smoothing; + double max_depth_change_factor; + double normal_smoothing_size; + // Normal space sampling + bool use_normal_space_sampling; + int bins_per_axis; + int num_samples; +}; +} // namespace depth_odometry + +#endif // DEPTH_ODOMETRY_POINT_TO_PLANE_ICP_DEPTH_ODOMETRY_PARAMS_H_ diff --git a/localization/depth_odometry/include/depth_odometry/pose_with_covariance_and_correspondences.h b/localization/depth_odometry/include/depth_odometry/pose_with_covariance_and_correspondences.h new file mode 100644 index 0000000000..72d0056d28 --- /dev/null +++ b/localization/depth_odometry/include/depth_odometry/pose_with_covariance_and_correspondences.h @@ -0,0 +1,53 @@ +/* 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. + */ +#ifndef DEPTH_ODOMETRY_POSE_WITH_COVARIANCE_AND_CORRESPONDENCES_H_ +#define DEPTH_ODOMETRY_POSE_WITH_COVARIANCE_AND_CORRESPONDENCES_H_ + +#include +#include +#include +#include + +namespace depth_odometry { +struct PoseWithCovarianceAndCorrespondences { + PoseWithCovarianceAndCorrespondences(const localization_common::PoseWithCovariance& pose_with_covariance, + const point_cloud_common::ICPCorrespondences& correspondences, + const localization_common::Time source_time, + const localization_common::Time target_time) + : pose_with_covariance(pose_with_covariance), + depth_correspondences(correspondences.source_points, correspondences.target_points), + source_time(source_time), + target_time(target_time) {} + + PoseWithCovarianceAndCorrespondences(const localization_common::PoseWithCovariance& pose_with_covariance, + const localization_measurements::DepthCorrespondences& correspondences, + const localization_common::Time source_time, + const localization_common::Time target_time) + : pose_with_covariance(pose_with_covariance), + depth_correspondences(correspondences), + source_time(source_time), + target_time(target_time) {} + + localization_common::PoseWithCovariance pose_with_covariance; + localization_measurements::DepthCorrespondences depth_correspondences; + localization_common::Time source_time; + localization_common::Time target_time; +}; +} // namespace depth_odometry + +#endif // DEPTH_ODOMETRY_POSE_WITH_COVARIANCE_AND_CORRESPONDENCES_H_ diff --git a/localization/depth_odometry/include/depth_odometry/utilities.h b/localization/depth_odometry/include/depth_odometry/utilities.h new file mode 100644 index 0000000000..564d5448eb --- /dev/null +++ b/localization/depth_odometry/include/depth_odometry/utilities.h @@ -0,0 +1,40 @@ +/* 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. + */ +#ifndef DEPTH_ODOMETRY_UTILITIES_H_ +#define DEPTH_ODOMETRY_UTILITIES_H_ + +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace depth_odometry { +ff_msgs::DepthOdometry DepthOdometryMsg(const PoseWithCovarianceAndCorrespondences& sensor_F_source_T_target, + const localization_common::PoseWithCovariance& body_F_source_T_target, + const double runtime); +ff_msgs::Odometry OdometryMsg(const localization_common::PoseWithCovariance& sensor_F_source_T_target, + const localization_common::PoseWithCovariance& body_F_source_T_target); +std::vector CorrespondencesMsg( + const localization_measurements::DepthCorrespondences& depth_correspondences); +} // namespace depth_odometry +#endif // DEPTH_ODOMETRY_UTILITIES_H_ diff --git a/localization/depth_odometry/launch/depth_odometry.launch b/localization/depth_odometry/launch/depth_odometry.launch new file mode 100644 index 0000000000..57f35c7183 --- /dev/null +++ b/localization/depth_odometry/launch/depth_odometry.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/localization/depth_odometry/nodelet_plugins.xml b/localization/depth_odometry/nodelet_plugins.xml new file mode 100644 index 0000000000..04eccafc8d --- /dev/null +++ b/localization/depth_odometry/nodelet_plugins.xml @@ -0,0 +1,11 @@ + + + + This nodelet wraps depth_odometry + + + + + diff --git a/localization/depth_odometry/package.xml b/localization/depth_odometry/package.xml new file mode 100644 index 0000000000..0a1ec16137 --- /dev/null +++ b/localization/depth_odometry/package.xml @@ -0,0 +1,46 @@ + + depth_odometry + 1.0.0 + + The depth odometry package + + + Apache License, Version 2.0 + + + Astrobee Flight Software + + + Astrobee Flight Software + + catkin + camera + ff_msgs + ff_util + localization_common + localization_measurements + msg_conversions + nodelet + optimization_common + pcl_conversions + pcl + point_cloud_common + vision_common + roscpp + camera + ff_msgs + ff_util + localization_common + localization_measurements + msg_conversions + nodelet + optimization_common + pcl_conversions + pcl + point_cloud_common + vision_common + roscpp + + + + diff --git a/localization/depth_odometry/readme.md b/localization/depth_odometry/readme.md new file mode 100644 index 0000000000..a3ff0a1ae3 --- /dev/null +++ b/localization/depth_odometry/readme.md @@ -0,0 +1,25 @@ +\page depthodometry Depth Odometry + +# Package Overview + +### DepthOdometryNodelet +The DepthOdometryNodlet subscribes to ROS messages for online use and publishes DepthOdometry messages. + +### DepthOdometryWrapper +The DepthOdometryWrapper joins input point clouds and intensity images with the same timestamp and passes the resulting depth image measurement to the chosen DepthOdometry class. + +### DepthOdometry +Base class for performing depth odometry using input depth image measurements. Child classes include ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry and PointToPlaneICPDepthOdometry. + +### ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry +Tracks image features between successive depth image measurements using only the intensity images of the measurements and performs alignement with the correspondending 3d points. Supports brisk, surf, and lucas kanade optical flow tracking and optimization-based point to point, point to plane, and symmetric point to plane 3d point aligment. See the point\_cloud\_common package for more details on aligment options and the vision\_common package for more details on image feature tracking options. + +### PointToPlaneICPDepthOdometry +Utilizes a chosen variant of point to plane ICP (nonsymmetric cost, symmetric cost, coarse to fine, etc.) to align successive depth image measurements. See the point\_cloud\_common package for more details on the point to plane ICP options. + +# Inputs +* `/hw/depth\_haz/extended/amplitude\_int` +* `/hw/depth\_haz/points` + +# Outputs +* `/loc/depth/odometry` diff --git a/localization/depth_odometry/src/depth_odometry_nodelet.cc b/localization/depth_odometry/src/depth_odometry_nodelet.cc new file mode 100644 index 0000000000..bfd806b073 --- /dev/null +++ b/localization/depth_odometry/src/depth_odometry_nodelet.cc @@ -0,0 +1,73 @@ +/* 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. + */ + +#include +#include +#include +#include +#include + +namespace depth_odometry { +namespace lc = localization_common; +namespace mc = msg_conversions; + +DepthOdometryNodelet::DepthOdometryNodelet() : ff_util::FreeFlyerNodelet(NODE_DEPTH_ODOM, true), enabled_(false) {} + +void DepthOdometryNodelet::Initialize(ros::NodeHandle* nh) { SubscribeAndAdvertise(nh); } + +void DepthOdometryNodelet::SubscribeAndAdvertise(ros::NodeHandle* nh) { + const std::string point_cloud_topic = static_cast(TOPIC_HARDWARE_PICOFLEXX_PREFIX) + + static_cast(TOPIC_HARDWARE_NAME_HAZ_CAM) + + static_cast(TOPIC_HARDWARE_PICOFLEXX_SUFFIX); + point_cloud_sub_ = nh->subscribe( + point_cloud_topic, 1, &DepthOdometryNodelet::PointCloudCallback, this, ros::TransportHints().tcpNoDelay()); + + image_transport::ImageTransport image_transport(*nh); + const std::string image_topic = static_cast(TOPIC_HARDWARE_PICOFLEXX_PREFIX) + + static_cast(TOPIC_HARDWARE_NAME_HAZ_CAM) + + static_cast(TOPIC_HARDWARE_PICOFLEXX_SUFFIX_EXTENDED) + + static_cast(TOPIC_HARDWARE_PICOFLEXX_SUFFIX_AMPLITUDE_IMAGE); + image_sub_ = image_transport.subscribe(image_topic, 1, &DepthOdometryNodelet::ImageCallback, this); + depth_odometry_pub_ = nh->advertise(TOPIC_LOCALIZATION_DEPTH_ODOM, 10); + enable_srv_ = nh->advertiseService(SERVICE_LOCALIZATION_DO_ENABLE, &DepthOdometryNodelet::EnableService, this); +} + +void DepthOdometryNodelet::PointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& point_cloud_msg) { + if (!enabled_) return; + const auto depth_odometry_msgs = depth_odometry_wrapper_.PointCloudCallback(point_cloud_msg); + for (const auto& depth_odometry_msg : depth_odometry_msgs) { + depth_odometry_pub_.publish(depth_odometry_msg); + } +} + +void DepthOdometryNodelet::ImageCallback(const sensor_msgs::ImageConstPtr& image_msg) { + if (!enabled_) return; + const auto depth_odometry_msgs = depth_odometry_wrapper_.ImageCallback(image_msg); + for (const auto& depth_odometry_msg : depth_odometry_msgs) { + depth_odometry_pub_.publish(depth_odometry_msg); + } +} + +bool DepthOdometryNodelet::EnableService(ff_msgs::SetBool::Request& req, ff_msgs::SetBool::Response& res) { + enabled_ = req.enable; + res.success = true; + return true; +} +} // namespace depth_odometry + +PLUGINLIB_EXPORT_CLASS(depth_odometry::DepthOdometryNodelet, nodelet::Nodelet); diff --git a/localization/depth_odometry/src/depth_odometry_wrapper.cc b/localization/depth_odometry/src/depth_odometry_wrapper.cc new file mode 100644 index 0000000000..e55211c820 --- /dev/null +++ b/localization/depth_odometry/src/depth_odometry_wrapper.cc @@ -0,0 +1,115 @@ +/* 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. + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace depth_odometry { +namespace lc = localization_common; +namespace lm = localization_measurements; +namespace mc = msg_conversions; + +DepthOdometryWrapper::DepthOdometryWrapper(const std::string& config_prefix) { + config_reader::ConfigReader config; + config.AddFile("cameras.config"); + config.AddFile("transforms.config"); + config.AddFile("geometry.config"); + config.AddFile((config_prefix + "depth_odometry.config").c_str()); + if (!config.ReadFiles()) { + LogFatal("Failed to read config files."); + } + DepthOdometryWrapperParams params; + LoadDepthOdometryWrapperParams(config, params); + Initialize(params); +} + +DepthOdometryWrapper::DepthOdometryWrapper(const DepthOdometryWrapperParams& params) { Initialize(params); } + +void DepthOdometryWrapper::Initialize(const DepthOdometryWrapperParams& params) { + params_ = params; + if (params_.method == "icp") { + depth_odometry_.reset(new PointToPlaneICPDepthOdometry(params.icp)); + } else if (params_.method == "image_feature") { + depth_odometry_.reset(new ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry(params.image_features)); + } else { + LogFatal("DepthOdometryWrapper: Invalid depth odometry method selected."); + } +} + +std::vector DepthOdometryWrapper::PointCloudCallback( + const sensor_msgs::PointCloud2ConstPtr& point_cloud_msg) { + point_cloud_buffer_.Add(lc::TimeFromHeader(point_cloud_msg->header), point_cloud_msg); + return ProcessDepthImageIfAvailable(); +} + +std::vector DepthOdometryWrapper::ImageCallback(const sensor_msgs::ImageConstPtr& image_msg) { + image_buffer_.Add(lc::TimeFromHeader(image_msg->header), image_msg); + return ProcessDepthImageIfAvailable(); +} + +std::vector DepthOdometryWrapper::ProcessDepthImageIfAvailable() { + std::vector depth_image_measurements; + boost::optional latest_added_point_cloud_msg_time; + boost::optional latest_added_image_msg_time; + // Point clouds and depth images for the same measurement arrive on different topics. + // Correlate pairs of these if possible. + for (const auto& image_msg : image_buffer_.measurements()) { + const auto image_msg_timestamp = image_msg.first; + const auto point_cloud_msg = + point_cloud_buffer_.GetNearby(image_msg_timestamp, params_.max_image_and_point_cloud_time_diff); + if (point_cloud_msg) { + const auto depth_image_measurement = + lm::MakeDepthImageMeasurement(*point_cloud_msg, image_msg.second, params_.haz_cam_A_haz_depth); + if (!depth_image_measurement) { + LogError("ProcessDepthImageIfAvailable: Failed to create depth image measurement."); + continue; + } + depth_image_measurements.emplace_back(*depth_image_measurement); + latest_added_point_cloud_msg_time = lc::TimeFromHeader((*point_cloud_msg)->header); + latest_added_image_msg_time = image_msg_timestamp; + } + } + + if (latest_added_point_cloud_msg_time) point_cloud_buffer_.EraseUpToAndIncluding(*latest_added_point_cloud_msg_time); + if (latest_added_image_msg_time) image_buffer_.EraseUpToAndIncluding(*latest_added_image_msg_time); + + std::vector depth_odometry_msgs; + for (const auto& depth_image_measurement : depth_image_measurements) { + timer_.Start(); + auto sensor_F_source_T_target = depth_odometry_->DepthImageCallback(depth_image_measurement); + timer_.Stop(); + if (sensor_F_source_T_target) { + const lc::PoseWithCovariance body_F_source_T_target = lc::FrameChangeRelativePoseWithCovariance( + sensor_F_source_T_target->pose_with_covariance, params_.body_T_haz_cam); + ff_msgs::DepthOdometry depth_odometry_msg = + DepthOdometryMsg(*sensor_F_source_T_target, body_F_source_T_target, timer_.last_value()); + depth_odometry_msgs.emplace_back(depth_odometry_msg); + } + } + return depth_odometry_msgs; +} +} // namespace depth_odometry diff --git a/localization/depth_odometry/src/image_features_with_known_correspondences_aligner_depth_odometry.cc b/localization/depth_odometry/src/image_features_with_known_correspondences_aligner_depth_odometry.cc new file mode 100644 index 0000000000..4357a913a1 --- /dev/null +++ b/localization/depth_odometry/src/image_features_with_known_correspondences_aligner_depth_odometry.cc @@ -0,0 +1,182 @@ +/* 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. + */ + +#include +#include +#include +#include +#include +#include +#include + +namespace depth_odometry { +namespace lc = localization_common; +namespace lm = localization_measurements; +namespace pc = point_cloud_common; +namespace vc = vision_common; + +ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry::ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry( + const ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams& params) + : params_(params), aligner_(params.aligner) { + if (params_.detector == "brisk") { + feature_detector_and_matcher_.reset( + new vc::BriskFeatureDetectorAndMatcher(params_.brisk_feature_detector_and_matcher)); + } else if (params_.detector == "lk_optical_flow") { + feature_detector_and_matcher_.reset( + new vc::LKOpticalFlowFeatureDetectorAndMatcher(params_.lk_optical_flow_feature_detector_and_matcher)); + } else if (params_.detector == "surf") { + feature_detector_and_matcher_.reset( + new vc::SurfFeatureDetectorAndMatcher(params_.surf_feature_detector_and_matcher)); + } else { + LogFatal("ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry: Invalid feature detector and matcher."); + } + + if (params_.use_clahe) + clahe_ = cv::createCLAHE(params_.clahe_clip_limit, cv::Size(params_.clahe_grid_length, params_.clahe_grid_length)); + + normals_required_ = params_.aligner.use_point_to_plane_cost || params_.aligner.use_symmetric_point_to_plane_cost; + + if (params_.refine_estimate) { + point_to_plane_icp_depth_odometry_ = PointToPlaneICPDepthOdometry(params_.point_to_plane_icp); + } +} + +boost::optional +ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry::DepthImageCallback( + const lm::DepthImageMeasurement& depth_image_measurement) { + if (!previous_depth_image_features_and_points_ && !latest_depth_image_features_and_points_) { + latest_depth_image_features_and_points_.reset(new DepthImageFeaturesAndPoints( + depth_image_measurement.depth_image, *(feature_detector_and_matcher_->detector()), clahe_, normals_required_)); + latest_timestamp_ = depth_image_measurement.timestamp; + if (params_.refine_estimate) point_to_plane_icp_depth_odometry_->DepthImageCallback(depth_image_measurement); + return boost::none; + } + const lc::Time timestamp = depth_image_measurement.timestamp; + if (timestamp < latest_timestamp_) { + LogWarning("DepthImageCallback: Out of order measurement received."); + return boost::none; + } + + previous_depth_image_features_and_points_ = latest_depth_image_features_and_points_; + previous_timestamp_ = latest_timestamp_; + latest_depth_image_features_and_points_.reset(new DepthImageFeaturesAndPoints( + depth_image_measurement.depth_image, *(feature_detector_and_matcher_->detector()), clahe_, normals_required_)); + latest_timestamp_ = timestamp; + + const double time_diff = latest_timestamp_ - previous_timestamp_; + if (time_diff > params_.max_time_diff) { + LogWarning("DepthImageCallback: Time difference too large, time diff: " << time_diff); + return boost::none; + } + + const auto& matches = feature_detector_and_matcher_->Match(previous_depth_image_features_and_points_->feature_image(), + latest_depth_image_features_and_points_->feature_image()); + // Get 3d points and required normals for matches + // Continue if any of these, including image points, are invalid + std::vector source_image_points; + std::vector target_image_points; + std::vector source_landmarks; + std::vector target_landmarks; + std::vector source_normals; + std::vector target_normals; + for (int i = 0; i < static_cast(matches.size()); ++i) { + const auto& match = matches[i]; + const auto& source_image_point = match.source_point; + const auto& target_image_point = match.target_point; + if (!ValidImagePoint(source_image_point) || !ValidImagePoint(target_image_point)) continue; + const auto source_point_3d = previous_depth_image_features_and_points_->depth_image().InterpolatePoint3D( + source_image_point.x(), source_image_point.y()); + const auto target_point_3d = latest_depth_image_features_and_points_->depth_image().InterpolatePoint3D( + target_image_point.x(), target_image_point.y()); + if (!Valid3dPoint(source_point_3d) || !Valid3dPoint(target_point_3d)) continue; + const Eigen::Vector3d source_landmark = pc::Vector3d(*source_point_3d); + const Eigen::Vector3d target_landmark = pc::Vector3d(*target_point_3d); + if (normals_required_) { + const auto target_normal = + latest_depth_image_features_and_points_->Normal(target_landmark, params_.aligner.normal_search_radius); + if (!target_normal) continue; + if (params_.aligner.use_symmetric_point_to_plane_cost) { + const auto source_normal = + previous_depth_image_features_and_points_->Normal(source_landmark, params_.aligner.normal_search_radius); + if (!source_normal) continue; + source_normals.emplace_back(*source_normal); + } + target_normals.emplace_back(*target_normal); + } + + source_image_points.emplace_back(source_image_point); + target_image_points.emplace_back(target_image_point); + source_landmarks.emplace_back(source_landmark); + target_landmarks.emplace_back(target_landmark); + } + + if (target_landmarks.size() < 4) { + LogError("DepthImageCallback: Too few points provided, need 4 but given " << target_landmarks.size() << "."); + return boost::none; + } + + // TODO(rsoussan): This isn't required with std:optional, remove when upgrade to c++17 and change normals + // containers to be boost::optional types + boost::optional&> source_normals_ref = + normals_required_ && params_.aligner.use_symmetric_point_to_plane_cost + ? boost::optional&>(source_normals) + : boost::none; + boost::optional&> target_normals_ref = + normals_required_ ? boost::optional&>(target_normals) : boost::none; + + auto target_T_source = + aligner_.ComputeRelativeTransform(source_landmarks, target_landmarks, source_normals_ref, target_normals_ref); + if (!target_T_source) { + LogWarning("DepthImageCallback: Failed to get relative transform."); + return boost::none; + } + + if (params_.refine_estimate) { + return point_to_plane_icp_depth_odometry_->DepthImageCallbackWithEstimate(depth_image_measurement, + target_T_source->pose); + } + + const auto source_T_target = lc::InvertPoseWithCovariance(*target_T_source); + + if (!lc::PoseCovarianceSane(source_T_target.covariance, params_.position_covariance_threshold, + params_.orientation_covariance_threshold)) { + LogWarning("DepthImageCallback: Sanity check failed - invalid covariance."); + return boost::none; + } + + return PoseWithCovarianceAndCorrespondences( + source_T_target, + lm::DepthCorrespondences(source_image_points, target_image_points, source_landmarks, target_landmarks), + previous_timestamp_, latest_timestamp_); +} + +bool ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry::ValidImagePoint( + const Eigen::Vector2d& image_point) const { + const int cols = latest_depth_image_features_and_points_->feature_image().cols(); + const int rows = latest_depth_image_features_and_points_->feature_image().rows(); + const double x_distance_to_border = std::min(image_point.x(), cols - image_point.x()); + const double y_distance_to_border = std::min(image_point.y(), rows - image_point.y()); + return (x_distance_to_border >= params_.min_x_distance_to_border && + y_distance_to_border >= params_.min_y_distance_to_border); +} + +bool ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry::Valid3dPoint( + const boost::optional& point) const { + return point && pc::ValidPoint(*point) && point->z >= 0; +} +} // namespace depth_odometry diff --git a/localization/depth_odometry/src/parameter_reader.cc b/localization/depth_odometry/src/parameter_reader.cc new file mode 100644 index 0000000000..e423eac927 --- /dev/null +++ b/localization/depth_odometry/src/parameter_reader.cc @@ -0,0 +1,92 @@ +/* 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. + */ + +#include +#include +#include +#include +#include +#include + +namespace depth_odometry { +namespace mc = msg_conversions; +namespace pc = point_cloud_common; +namespace vc = vision_common; + +void LoadDepthOdometryWrapperParams(config_reader::ConfigReader& config, DepthOdometryWrapperParams& params) { + params.max_image_and_point_cloud_time_diff = mc::LoadDouble(config, "max_image_and_point_cloud_time_diff"); + params.method = mc::LoadString(config, "depth_odometry_method"); + params.body_T_haz_cam = msg_conversions::LoadEigenTransform(config, "haz_cam_transform"); + params.haz_cam_A_haz_depth = Eigen::Affine3d::Identity(); + LoadPointToPlaneICPDepthOdometryParams(config, params.icp); + LoadImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams(config, params.image_features); +} + +void LoadDepthOdometryParams(config_reader::ConfigReader& config, DepthOdometryParams& params) { + params.max_time_diff = mc::LoadDouble(config, "max_time_diff"); + params.position_covariance_threshold = mc::LoadDouble(config, "position_covariance_threshold"); + params.orientation_covariance_threshold = mc::LoadDouble(config, "orientation_covariance_threshold"); +} + +void LoadPointToPlaneICPDepthOdometryParams(config_reader::ConfigReader& config, + PointToPlaneICPDepthOdometryParams& params) { + pc::LoadPointToPlaneICPParams(config, params.icp); + params.downsample = mc::LoadBool(config, "downsample"); + params.downsample_leaf_size = mc::LoadDouble(config, "downsample_leaf_size"); + params.use_organized_normal_estimation = mc::LoadBool(config, "use_organized_normal_estimation"); + const std::string normal_estimation_method_name = mc::LoadString(config, "organized_normal_method"); + if (normal_estimation_method_name == "avg_3d_gradient") { + params.normal_estimation_method = + pcl::IntegralImageNormalEstimation::NormalEstimationMethod::AVERAGE_3D_GRADIENT; + } else if (normal_estimation_method_name == "covariance") { + params.normal_estimation_method = + pcl::IntegralImageNormalEstimation::NormalEstimationMethod::COVARIANCE_MATRIX; + } else if (normal_estimation_method_name == "avg_depth_change") { + params.normal_estimation_method = + pcl::IntegralImageNormalEstimation::NormalEstimationMethod::AVERAGE_DEPTH_CHANGE; + } else { + LogFatal("LoadPointToPlaneICPDepthOdometryParams: Invalid normal estimation method provided."); + } + params.normal_smoothing_size = mc::LoadDouble(config, "normal_smoothing_size"); + params.use_depth_dependent_smoothing = mc::LoadBool(config, "use_depth_dependent_smoothing"); + params.max_depth_change_factor = mc::LoadDouble(config, "max_depth_change_factor"); + params.normal_smoothing_size = mc::LoadDouble(config, "normal_smoothing_size"); + params.use_normal_space_sampling = mc::LoadBool(config, "use_normal_space_sampling"); + params.bins_per_axis = mc::LoadInt(config, "bins_per_axis"); + params.num_samples = mc::LoadInt(config, "num_samples"); + LoadDepthOdometryParams(config, params); +} + +void LoadImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams( + config_reader::ConfigReader& config, ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams& params) { + pc::LoadPointCloudWithKnownCorrespondencesAlignerParams(config, params.aligner); + vc::LoadBriskFeatureDetectorAndMatcherParams(config, params.brisk_feature_detector_and_matcher); + vc::LoadLKOpticalFlowFeatureDetectorAndMatcherParams(config, params.lk_optical_flow_feature_detector_and_matcher); + vc::LoadSurfFeatureDetectorAndMatcherParams(config, params.surf_feature_detector_and_matcher); + params.detector = mc::LoadString(config, "detector"); + params.use_clahe = mc::LoadBool(config, "use_clahe"); + params.clahe_grid_length = mc::LoadInt(config, "clahe_grid_length"); + params.clahe_clip_limit = mc::LoadDouble(config, "clahe_clip_limit"); + params.min_x_distance_to_border = mc::LoadDouble(config, "min_x_distance_to_border"); + params.min_y_distance_to_border = mc::LoadDouble(config, "min_y_distance_to_border"); + params.min_num_inliers = mc::LoadInt(config, "min_num_inliers"); + params.refine_estimate = mc::LoadBool(config, "refine_estimate"); + if (params.refine_estimate) LoadPointToPlaneICPDepthOdometryParams(config, params.point_to_plane_icp); + LoadDepthOdometryParams(config, params); +} +} // namespace depth_odometry diff --git a/localization/depth_odometry/src/point_to_plane_icp_depth_odometry.cc b/localization/depth_odometry/src/point_to_plane_icp_depth_odometry.cc new file mode 100644 index 0000000000..5a361c8b74 --- /dev/null +++ b/localization/depth_odometry/src/point_to_plane_icp_depth_odometry.cc @@ -0,0 +1,130 @@ +/* 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. + */ + +#include +#include +#include +#include + +namespace depth_odometry { +namespace lc = localization_common; +namespace lm = localization_measurements; +namespace pc = point_cloud_common; + +PointToPlaneICPDepthOdometry::PointToPlaneICPDepthOdometry(const PointToPlaneICPDepthOdometryParams& params) + : params_(params), icp_(params.icp) {} + +pcl::PointCloud::Ptr PointToPlaneICPDepthOdometry::DownsampleAndFilterCloud( + const pcl::PointCloud::Ptr& cloud) const { + pcl::PointCloud::Ptr filtered_cloud_with_normals(new pcl::PointCloud()); + if (params_.use_organized_normal_estimation) { + pcl::PointCloud::Ptr filtered_cloud(new pcl::PointCloud(*cloud)); + pc::ReplaceZerosWithNans(*filtered_cloud); + pc::EstimateOrganizedNormals( + filtered_cloud, params_.normal_estimation_method, params_.use_depth_dependent_smoothing, + params_.max_depth_change_factor, params_.normal_smoothing_size, *filtered_cloud_with_normals); + pc::RemoveInvalidAndZeroPoints(*filtered_cloud_with_normals); + // Downsample after estimating organized normals since organized normal estimation applies some smoothing and is + // fast enough to use before downsampling (and more accurate before downsampling) + if (params_.downsample) { + filtered_cloud_with_normals = + pc::DownsamplePointCloud(filtered_cloud_with_normals, params_.downsample_leaf_size); + } + } else { + if (params_.downsample) { + const auto downsampled_cloud = pc::DownsamplePointCloud(cloud, params_.downsample_leaf_size); + filtered_cloud_with_normals = pc::FilteredPointCloudWithNormals( + downsampled_cloud, params_.icp.search_radius); + } else { + filtered_cloud_with_normals = + pc::FilteredPointCloudWithNormals(cloud, params_.icp.search_radius); + } + } + if (params_.use_normal_space_sampling) { + pc::NormalSpaceSubsampling(filtered_cloud_with_normals, params_.bins_per_axis, + params_.num_samples); + } + return filtered_cloud_with_normals; +} + +boost::optional PointToPlaneICPDepthOdometry::DepthImageCallback( + const lm::DepthImageMeasurement& depth_image_measurement) { + return DepthImageCallbackWithEstimate(depth_image_measurement); +} + +boost::optional PointToPlaneICPDepthOdometry::DepthImageCallbackWithEstimate( + const localization_measurements::DepthImageMeasurement& depth_image_measurement, + const boost::optional target_T_source_initial_estimate) { + if (!previous_point_cloud_with_normals_ && !latest_point_cloud_with_normals_) { + latest_point_cloud_with_normals_ = + DownsampleAndFilterCloud(depth_image_measurement.depth_image.unfiltered_point_cloud()); + latest_timestamp_ = depth_image_measurement.timestamp; + return boost::none; + } + const lc::Time timestamp = depth_image_measurement.timestamp; + if (timestamp < latest_timestamp_) { + LogWarning("DepthImageCallback: Out of order measurement received."); + return boost::none; + } + + previous_point_cloud_with_normals_ = latest_point_cloud_with_normals_; + previous_timestamp_ = latest_timestamp_; + latest_point_cloud_with_normals_ = + DownsampleAndFilterCloud(depth_image_measurement.depth_image.unfiltered_point_cloud()); + latest_timestamp_ = timestamp; + + const double time_diff = latest_timestamp_ - previous_timestamp_; + if (time_diff > params_.max_time_diff) { + LogWarning("DepthImageCallback: Time difference too large, time diff: " << time_diff); + return boost::none; + } + + if (target_T_source_initial_estimate) { + pcl::transformPointCloudWithNormals(*previous_point_cloud_with_normals_, *previous_point_cloud_with_normals_, + target_T_source_initial_estimate->matrix()); + } + auto target_T_source = + icp_.ComputeRelativeTransform(previous_point_cloud_with_normals_, latest_point_cloud_with_normals_); + if (!target_T_source) { + LogWarning("DepthImageCallback: Failed to get relative transform."); + return boost::none; + } + + if (target_T_source_initial_estimate) { + target_T_source->pose = target_T_source->pose * *target_T_source_initial_estimate; + // TODO(rsoussan): Frame change covariance! + } + + const auto source_T_target = lc::InvertPoseWithCovariance(*target_T_source); + + if (!lc::PoseCovarianceSane(source_T_target.covariance, params_.position_covariance_threshold, + params_.orientation_covariance_threshold)) { + LogWarning("DepthImageCallback: Sanity check failed - invalid covariance."); + return boost::none; + } + + const auto correspondences = icp_.correspondences(); + if (!correspondences) { + LogWarning("DepthImageCallback: Failed to get correspondences."); + return boost::none; + } + + return PoseWithCovarianceAndCorrespondences(source_T_target, *correspondences, previous_timestamp_, + latest_timestamp_); +} +} // namespace depth_odometry diff --git a/localization/depth_odometry/src/utilities.cc b/localization/depth_odometry/src/utilities.cc new file mode 100644 index 0000000000..a17048e8e7 --- /dev/null +++ b/localization/depth_odometry/src/utilities.cc @@ -0,0 +1,74 @@ +/* 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. + */ + +#include +#include +#include + +namespace depth_odometry { +namespace lc = localization_common; +namespace lm = localization_measurements; +namespace mc = msg_conversions; + +ff_msgs::DepthOdometry DepthOdometryMsg(const PoseWithCovarianceAndCorrespondences& sensor_F_source_T_target, + const lc::PoseWithCovariance& body_F_source_T_target, const double runtime) { + ff_msgs::DepthOdometry depth_odometry_msg; + depth_odometry_msg.odometry = OdometryMsg(sensor_F_source_T_target.pose_with_covariance, body_F_source_T_target); + depth_odometry_msg.correspondences = CorrespondencesMsg(sensor_F_source_T_target.depth_correspondences); + depth_odometry_msg.valid_image_points = sensor_F_source_T_target.depth_correspondences.valid_image_points; + depth_odometry_msg.valid_points_3d = sensor_F_source_T_target.depth_correspondences.valid_3d_points; + lc::TimeToHeader(sensor_F_source_T_target.target_time, depth_odometry_msg.header); + lc::TimeToMsg(sensor_F_source_T_target.source_time, depth_odometry_msg.odometry.source_time); + lc::TimeToMsg(sensor_F_source_T_target.target_time, depth_odometry_msg.odometry.target_time); + depth_odometry_msg.runtime = runtime; + depth_odometry_msg.header.frame_id = "haz_cam"; + return depth_odometry_msg; +} + +ff_msgs::Odometry OdometryMsg(const lc::PoseWithCovariance& sensor_F_source_T_target, + const lc::PoseWithCovariance& body_F_source_T_target) { + ff_msgs::Odometry odometry_msg; + mc::EigenPoseCovarianceToMsg(sensor_F_source_T_target.pose, sensor_F_source_T_target.covariance, + odometry_msg.sensor_F_source_T_target); + mc::EigenPoseCovarianceToMsg(body_F_source_T_target.pose, body_F_source_T_target.covariance, + odometry_msg.body_F_source_T_target); + return odometry_msg; +} + +std::vector CorrespondencesMsg(const lm::DepthCorrespondences& depth_correspondences) { + std::vector correspondences_msg; + int num_points = 0; + if (depth_correspondences.valid_3d_points) + num_points = depth_correspondences.source_3d_points.size(); + else if (depth_correspondences.valid_image_points) + num_points = depth_correspondences.source_image_points.size(); + for (int i = 0; i < num_points; ++i) { + ff_msgs::DepthCorrespondence correspondence; + if (depth_correspondences.valid_3d_points) { + mc::VectorToMsg(depth_correspondences.source_3d_points[i], correspondence.source_3d_point); + mc::VectorToMsg(depth_correspondences.target_3d_points[i], correspondence.target_3d_point); + } + if (depth_correspondences.valid_image_points) { + mc::Vector2dToMsg(depth_correspondences.source_image_points[i], correspondence.source_image_point); + mc::Vector2dToMsg(depth_correspondences.target_image_points[i], correspondence.target_image_point); + } + correspondences_msg.emplace_back(correspondence); + } + return correspondences_msg; +} +} // namespace depth_odometry diff --git a/localization/depth_odometry/test/test_depth_odometry_wrapper.cc b/localization/depth_odometry/test/test_depth_odometry_wrapper.cc new file mode 100644 index 0000000000..faae2b4786 --- /dev/null +++ b/localization/depth_odometry/test/test_depth_odometry_wrapper.cc @@ -0,0 +1,575 @@ +/* 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. + */ + +#include "test_utilities.h" // NOLINT +#include +#include +#include +#include +#include + +#include + +namespace dd = depth_odometry; +namespace lc = localization_common; + +TEST(DepthOdometryWrapperTester, PointToPlaneICP) { + auto params = dd::DefaultDepthOdometryWrapperParams(); + params.icp.icp.search_radius = 1.0; + dd::DepthOdometryWrapper depth_odometry_wrapper(params); + constexpr double translation_stddev = 0.01; + constexpr double rotation_stddev = 0.01; + // Add first measurement set + const lc::Time source_timestamp = 0; + const auto source_points_msg = dd::CubicPointsMsg(source_timestamp); + const auto source_image_msg = dd::ImageMsg(source_timestamp); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(source_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(source_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + // Add second measurement set + const lc::Time target_timestamp = 0.1; + const auto target_T_source = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_points_msg = dd::TransformPointsMsg(target_timestamp, source_points_msg, target_T_source); + const auto target_image_msg = dd::ImageMsg(target_timestamp); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(target_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(target_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 1); + const auto sensor_F_source_T_target = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.sensor_F_source_T_target.pose)); + EXPECT_PRED2(lc::MatrixEquality<4>, sensor_F_source_T_target.matrix(), target_T_source.inverse().matrix()); + const auto body_F_source_T_target = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.body_F_source_T_target.pose)); + const Eigen::Isometry3d true_body_F_source_T_target = + params.body_T_haz_cam * target_T_source.inverse() * (params.body_T_haz_cam).inverse(); + EXPECT_PRED2(lc::MatrixEquality<4>, body_F_source_T_target.matrix(), true_body_F_source_T_target.matrix()); + } + // Add third measurement set + const lc::Time target_2_timestamp = 0.1; + const auto target_2_T_target = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_2_points_msg = dd::TransformPointsMsg(target_2_timestamp, target_points_msg, target_2_T_target); + const auto target_2_image_msg = dd::ImageMsg(target_2_timestamp); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(target_2_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(target_2_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 1); + const auto sensor_F_target_T_target_2 = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.sensor_F_source_T_target.pose)); + EXPECT_PRED2(lc::MatrixEquality<4>, sensor_F_target_T_target_2.matrix(), target_2_T_target.inverse().matrix()); + const auto body_F_target_T_target_2 = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.body_F_source_T_target.pose)); + const Eigen::Isometry3d true_body_F_target_T_target_2 = + params.body_T_haz_cam * target_2_T_target.inverse() * (params.body_T_haz_cam).inverse(); + EXPECT_PRED2(lc::MatrixEquality<4>, body_F_target_T_target_2.matrix(), true_body_F_target_T_target_2.matrix()); + } +} + +TEST(DepthOdometryWrapperTester, SymmetricPointToPlaneICP) { + auto params = dd::DefaultDepthOdometryWrapperParams(); + params.icp.icp.search_radius = 1.0; + params.icp.icp.symmetric_objective = true; + dd::DepthOdometryWrapper depth_odometry_wrapper(params); + constexpr double translation_stddev = 0.01; + constexpr double rotation_stddev = 0.01; + // Add first measurement set + const lc::Time source_timestamp = 0; + const auto source_points_msg = dd::CubicPointsMsg(source_timestamp); + const auto source_image_msg = dd::ImageMsg(source_timestamp); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(source_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(source_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + // Add second measurement set + const lc::Time target_timestamp = 0.1; + const auto target_T_source = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_points_msg = dd::TransformPointsMsg(target_timestamp, source_points_msg, target_T_source); + const auto target_image_msg = dd::ImageMsg(target_timestamp); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(target_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(target_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 1); + const auto sensor_F_source_T_target = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.sensor_F_source_T_target.pose)); + EXPECT_PRED2(lc::MatrixEquality<4>, sensor_F_source_T_target.matrix(), target_T_source.inverse().matrix()); + const auto body_F_source_T_target = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.body_F_source_T_target.pose)); + const Eigen::Isometry3d true_body_F_source_T_target = + params.body_T_haz_cam * target_T_source.inverse() * (params.body_T_haz_cam).inverse(); + EXPECT_PRED2(lc::MatrixEquality<4>, body_F_source_T_target.matrix(), true_body_F_source_T_target.matrix()); + } + // Add third measurement set + const lc::Time target_2_timestamp = 0.1; + const auto target_2_T_target = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_2_points_msg = dd::TransformPointsMsg(target_2_timestamp, target_points_msg, target_2_T_target); + const auto target_2_image_msg = dd::ImageMsg(target_2_timestamp); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(target_2_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(target_2_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 1); + const auto sensor_F_target_T_target_2 = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.sensor_F_source_T_target.pose)); + EXPECT_PRED2(lc::MatrixEquality<4>, sensor_F_target_T_target_2.matrix(), target_2_T_target.inverse().matrix()); + const auto body_F_target_T_target_2 = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.body_F_source_T_target.pose)); + const Eigen::Isometry3d true_body_F_target_T_target_2 = + params.body_T_haz_cam * target_2_T_target.inverse() * (params.body_T_haz_cam).inverse(); + EXPECT_PRED2(lc::MatrixEquality<4>, body_F_target_T_target_2.matrix(), true_body_F_target_T_target_2.matrix()); + } +} + +TEST(DepthOdometryWrapperTester, CorrespondenceRejectorSymmetricPointToPlaneICP) { + auto params = dd::DefaultDepthOdometryWrapperParams(); + params.icp.icp.search_radius = 1.0; + params.icp.icp.symmetric_objective = true; + params.icp.icp.correspondence_rejector_surface_normal = true; + dd::DepthOdometryWrapper depth_odometry_wrapper(params); + constexpr double translation_stddev = 0.01; + constexpr double rotation_stddev = 0.01; + // Add first measurement set + const lc::Time source_timestamp = 0; + const auto source_points_msg = dd::CubicPointsMsg(source_timestamp); + const auto source_image_msg = dd::ImageMsg(source_timestamp); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(source_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(source_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + // Add second measurement set + const lc::Time target_timestamp = 0.1; + const auto target_T_source = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_points_msg = dd::TransformPointsMsg(target_timestamp, source_points_msg, target_T_source); + const auto target_image_msg = dd::ImageMsg(target_timestamp); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(target_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(target_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 1); + const auto sensor_F_source_T_target = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.sensor_F_source_T_target.pose)); + EXPECT_PRED2(lc::MatrixEquality<4>, sensor_F_source_T_target.matrix(), target_T_source.inverse().matrix()); + const auto body_F_source_T_target = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.body_F_source_T_target.pose)); + const Eigen::Isometry3d true_body_F_source_T_target = + params.body_T_haz_cam * target_T_source.inverse() * (params.body_T_haz_cam).inverse(); + EXPECT_PRED2(lc::MatrixEquality<4>, body_F_source_T_target.matrix(), true_body_F_source_T_target.matrix()); + } + // Add third measurement set + const lc::Time target_2_timestamp = 0.1; + const auto target_2_T_target = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_2_points_msg = dd::TransformPointsMsg(target_2_timestamp, target_points_msg, target_2_T_target); + const auto target_2_image_msg = dd::ImageMsg(target_2_timestamp); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(target_2_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(target_2_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 1); + const auto sensor_F_target_T_target_2 = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.sensor_F_source_T_target.pose)); + EXPECT_PRED2(lc::MatrixEquality<4>, sensor_F_target_T_target_2.matrix(), target_2_T_target.inverse().matrix()); + const auto body_F_target_T_target_2 = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.body_F_source_T_target.pose)); + const Eigen::Isometry3d true_body_F_target_T_target_2 = + params.body_T_haz_cam * target_2_T_target.inverse() * (params.body_T_haz_cam).inverse(); + EXPECT_PRED2(lc::MatrixEquality<4>, body_F_target_T_target_2.matrix(), true_body_F_target_T_target_2.matrix()); + } +} + +TEST(DepthOdometryWrapperTester, ImageFeatureAligner) { + auto params = dd::DefaultDepthOdometryWrapperParams(); + params.method = "image_feature"; + dd::DepthOdometryWrapper depth_odometry_wrapper(params); + constexpr double translation_stddev = 1; + constexpr double rotation_stddev = 1; + const cv::Point2i offset(5, 5); + // Add first measurement set + const lc::Time source_timestamp = 0; + const auto source_points_msg = dd::RampedPointsMsg(source_timestamp); + const auto source_image_msg = dd::MarkerImageMsg(source_timestamp); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(source_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(source_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + // Add second measurement set + const lc::Time target_timestamp = 0.1; + const auto target_T_source = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_points_msg = + dd::OffsetAndTransformPointsMsg(target_timestamp, source_points_msg, offset, target_T_source); + const auto target_image_msg = dd::MarkerImageMsg(target_timestamp, offset); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(target_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(target_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 1); + const auto sensor_F_source_T_target = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.sensor_F_source_T_target.pose)); + EXPECT_PRED2(lc::MatrixEquality<2>, sensor_F_source_T_target.matrix(), target_T_source.inverse().matrix()); + const auto body_F_source_T_target = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.body_F_source_T_target.pose)); + const Eigen::Isometry3d true_body_F_source_T_target = + params.body_T_haz_cam * target_T_source.inverse() * (params.body_T_haz_cam).inverse(); + EXPECT_PRED2(lc::MatrixEquality<2>, body_F_source_T_target.matrix(), true_body_F_source_T_target.matrix()); + } + // Add third measurement set + const lc::Time target_2_timestamp = 0.1; + const auto target_2_T_target = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_2_points_msg = + dd::OffsetAndTransformPointsMsg(target_2_timestamp, target_points_msg, offset, target_2_T_target); + const auto target_2_image_msg = dd::MarkerImageMsg(target_2_timestamp, offset * 2); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(target_2_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(target_2_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 1); + const auto sensor_F_target_T_target_2 = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.sensor_F_source_T_target.pose)); + EXPECT_PRED2(lc::MatrixEquality<2>, sensor_F_target_T_target_2.matrix(), target_2_T_target.inverse().matrix()); + const auto body_F_target_T_target_2 = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.body_F_source_T_target.pose)); + const Eigen::Isometry3d true_body_F_target_T_target_2 = + params.body_T_haz_cam * target_2_T_target.inverse() * (params.body_T_haz_cam).inverse(); + EXPECT_PRED2(lc::MatrixEquality<2>, body_F_target_T_target_2.matrix(), true_body_F_target_T_target_2.matrix()); + } +} + +TEST(DepthOdometryWrapperTester, PointToPlaneImageFeatureAligner) { + auto params = dd::DefaultDepthOdometryWrapperParams(); + params.method = "image_feature"; + params.image_features.aligner.use_point_to_plane_cost = true; + params.image_features.aligner.normal_search_radius = 3.0; + params.image_features.position_covariance_threshold = 10; + params.image_features.orientation_covariance_threshold = 10; + dd::DepthOdometryWrapper depth_odometry_wrapper(params); + constexpr double translation_stddev = 1; + constexpr double rotation_stddev = 1; + const cv::Point2i offset(5, 5); + // Add first measurement set + const lc::Time source_timestamp = 0; + const auto source_points_msg = dd::RampedPointsMsg(source_timestamp); + const auto source_image_msg = dd::MarkerImageMsg(source_timestamp); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(source_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(source_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + // Add second measurement set + const lc::Time target_timestamp = 0.1; + const auto target_T_source = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_points_msg = + dd::OffsetAndTransformPointsMsg(target_timestamp, source_points_msg, offset, target_T_source); + const auto target_image_msg = dd::MarkerImageMsg(target_timestamp, offset); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(target_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(target_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 1); + const auto sensor_F_source_T_target = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.sensor_F_source_T_target.pose)); + EXPECT_PRED2(lc::MatrixEquality<2>, sensor_F_source_T_target.matrix(), target_T_source.inverse().matrix()); + const auto body_F_source_T_target = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.body_F_source_T_target.pose)); + const Eigen::Isometry3d true_body_F_source_T_target = + params.body_T_haz_cam * target_T_source.inverse() * (params.body_T_haz_cam).inverse(); + EXPECT_PRED2(lc::MatrixEquality<2>, body_F_source_T_target.matrix(), true_body_F_source_T_target.matrix()); + } + // Add third measurement set + const lc::Time target_2_timestamp = 0.1; + const auto target_2_T_target = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_2_points_msg = + dd::OffsetAndTransformPointsMsg(target_2_timestamp, target_points_msg, offset, target_2_T_target); + const auto target_2_image_msg = dd::MarkerImageMsg(target_2_timestamp, offset * 2); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(target_2_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(target_2_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 1); + const auto sensor_F_target_T_target_2 = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.sensor_F_source_T_target.pose)); + EXPECT_PRED2(lc::MatrixEquality<2>, sensor_F_target_T_target_2.matrix(), target_2_T_target.inverse().matrix()); + const auto body_F_target_T_target_2 = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.body_F_source_T_target.pose)); + const Eigen::Isometry3d true_body_F_target_T_target_2 = + params.body_T_haz_cam * target_2_T_target.inverse() * (params.body_T_haz_cam).inverse(); + EXPECT_PRED2(lc::MatrixEquality<2>, body_F_target_T_target_2.matrix(), true_body_F_target_T_target_2.matrix()); + } +} + +TEST(DepthOdometryWrapperTester, SymmetricPointToPlaneImageFeatureAligner) { + auto params = dd::DefaultDepthOdometryWrapperParams(); + params.method = "image_feature"; + params.image_features.aligner.use_symmetric_point_to_plane_cost = true; + params.image_features.aligner.normal_search_radius = 3.0; + params.image_features.position_covariance_threshold = 10; + params.image_features.orientation_covariance_threshold = 10; + dd::DepthOdometryWrapper depth_odometry_wrapper(params); + constexpr double translation_stddev = 1; + constexpr double rotation_stddev = 1; + const cv::Point2i offset(5, 5); + // Add first measurement set + const lc::Time source_timestamp = 0; + const auto source_points_msg = dd::RampedPointsMsg(source_timestamp); + const auto source_image_msg = dd::MarkerImageMsg(source_timestamp); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(source_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(source_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + // Add second measurement set + const lc::Time target_timestamp = 0.1; + const auto target_T_source = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_points_msg = + dd::OffsetAndTransformPointsMsg(target_timestamp, source_points_msg, offset, target_T_source); + const auto target_image_msg = dd::MarkerImageMsg(target_timestamp, offset); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(target_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(target_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 1); + const auto sensor_F_source_T_target = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.sensor_F_source_T_target.pose)); + EXPECT_PRED2(lc::MatrixEquality<2>, sensor_F_source_T_target.matrix(), target_T_source.inverse().matrix()); + const auto body_F_source_T_target = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.body_F_source_T_target.pose)); + const Eigen::Isometry3d true_body_F_source_T_target = + params.body_T_haz_cam * target_T_source.inverse() * (params.body_T_haz_cam).inverse(); + EXPECT_PRED2(lc::MatrixEquality<2>, body_F_source_T_target.matrix(), true_body_F_source_T_target.matrix()); + } + // Add third measurement set + const lc::Time target_2_timestamp = 0.1; + const auto target_2_T_target = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_2_points_msg = + dd::OffsetAndTransformPointsMsg(target_2_timestamp, target_points_msg, offset, target_2_T_target); + const auto target_2_image_msg = dd::MarkerImageMsg(target_2_timestamp, offset * 2); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(target_2_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(target_2_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 1); + const auto sensor_F_target_T_target_2 = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.sensor_F_source_T_target.pose)); + EXPECT_PRED2(lc::MatrixEquality<2>, sensor_F_target_T_target_2.matrix(), target_2_T_target.inverse().matrix()); + const auto body_F_target_T_target_2 = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.body_F_source_T_target.pose)); + const Eigen::Isometry3d true_body_F_target_T_target_2 = + params.body_T_haz_cam * target_2_T_target.inverse() * (params.body_T_haz_cam).inverse(); + EXPECT_PRED2(lc::MatrixEquality<2>, body_F_target_T_target_2.matrix(), true_body_F_target_T_target_2.matrix()); + } +} + +TEST(DepthOdometryWrapperTester, SingleIterationUmeyamaImageFeatureAligner) { + auto params = dd::DefaultDepthOdometryWrapperParams(); + params.method = "image_feature"; + params.image_features.aligner.use_single_iteration_umeyama = true; + dd::DepthOdometryWrapper depth_odometry_wrapper(params); + constexpr double translation_stddev = 1; + constexpr double rotation_stddev = 1; + const cv::Point2i offset(5, 5); + // Add first measurement set + const lc::Time source_timestamp = 0; + const auto source_points_msg = dd::RampedPointsMsg(source_timestamp); + const auto source_image_msg = dd::MarkerImageMsg(source_timestamp); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(source_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(source_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + // Add second measurement set + const lc::Time target_timestamp = 0.1; + const auto target_T_source = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_points_msg = + dd::OffsetAndTransformPointsMsg(target_timestamp, source_points_msg, offset, target_T_source); + const auto target_image_msg = dd::MarkerImageMsg(target_timestamp, offset); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(target_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(target_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 1); + const auto sensor_F_source_T_target = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.sensor_F_source_T_target.pose)); + EXPECT_PRED2(lc::MatrixEquality<2>, sensor_F_source_T_target.matrix(), target_T_source.inverse().matrix()); + const auto body_F_source_T_target = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.body_F_source_T_target.pose)); + const Eigen::Isometry3d true_body_F_source_T_target = + params.body_T_haz_cam * target_T_source.inverse() * (params.body_T_haz_cam).inverse(); + EXPECT_PRED2(lc::MatrixEquality<2>, body_F_source_T_target.matrix(), true_body_F_source_T_target.matrix()); + } + // Add third measurement set + const lc::Time target_2_timestamp = 0.1; + const auto target_2_T_target = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_2_points_msg = + dd::OffsetAndTransformPointsMsg(target_2_timestamp, target_points_msg, offset, target_2_T_target); + const auto target_2_image_msg = dd::MarkerImageMsg(target_2_timestamp, offset * 2); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(target_2_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(target_2_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 1); + const auto sensor_F_target_T_target_2 = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.sensor_F_source_T_target.pose)); + EXPECT_PRED2(lc::MatrixEquality<2>, sensor_F_target_T_target_2.matrix(), target_2_T_target.inverse().matrix()); + const auto body_F_target_T_target_2 = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.body_F_source_T_target.pose)); + const Eigen::Isometry3d true_body_F_target_T_target_2 = + params.body_T_haz_cam * target_2_T_target.inverse() * (params.body_T_haz_cam).inverse(); + EXPECT_PRED2(lc::MatrixEquality<2>, body_F_target_T_target_2.matrix(), true_body_F_target_T_target_2.matrix()); + } +} + +TEST(DepthOdometryWrapperTester, UmeyamaInitialGuessImageFeatureAligner) { + auto params = dd::DefaultDepthOdometryWrapperParams(); + params.method = "image_feature"; + params.image_features.aligner.use_umeyama_initial_guess = true; + dd::DepthOdometryWrapper depth_odometry_wrapper(params); + constexpr double translation_stddev = 1; + constexpr double rotation_stddev = 1; + const cv::Point2i offset(5, 5); + // Add first measurement set + const lc::Time source_timestamp = 0; + const auto source_points_msg = dd::RampedPointsMsg(source_timestamp); + const auto source_image_msg = dd::MarkerImageMsg(source_timestamp); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(source_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(source_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + // Add second measurement set + const lc::Time target_timestamp = 0.1; + const auto target_T_source = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_points_msg = + dd::OffsetAndTransformPointsMsg(target_timestamp, source_points_msg, offset, target_T_source); + const auto target_image_msg = dd::MarkerImageMsg(target_timestamp, offset); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(target_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(target_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 1); + const auto sensor_F_source_T_target = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.sensor_F_source_T_target.pose)); + EXPECT_PRED2(lc::MatrixEquality<2>, sensor_F_source_T_target.matrix(), target_T_source.inverse().matrix()); + const auto body_F_source_T_target = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.body_F_source_T_target.pose)); + const Eigen::Isometry3d true_body_F_source_T_target = + params.body_T_haz_cam * target_T_source.inverse() * (params.body_T_haz_cam).inverse(); + EXPECT_PRED2(lc::MatrixEquality<2>, body_F_source_T_target.matrix(), true_body_F_source_T_target.matrix()); + } + // Add third measurement set + const lc::Time target_2_timestamp = 0.1; + const auto target_2_T_target = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_2_points_msg = + dd::OffsetAndTransformPointsMsg(target_2_timestamp, target_points_msg, offset, target_2_T_target); + const auto target_2_image_msg = dd::MarkerImageMsg(target_2_timestamp, offset * 2); + { + const auto depth_odometry_msgs = depth_odometry_wrapper.PointCloudCallback(target_2_points_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 0); + } + { + const auto depth_odometry_msgs = depth_odometry_wrapper.ImageCallback(target_2_image_msg); + ASSERT_EQ(depth_odometry_msgs.size(), 1); + const auto sensor_F_target_T_target_2 = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.sensor_F_source_T_target.pose)); + EXPECT_PRED2(lc::MatrixEquality<2>, sensor_F_target_T_target_2.matrix(), target_2_T_target.inverse().matrix()); + const auto body_F_target_T_target_2 = + lc::EigenPose(lc::PoseFromMsg(depth_odometry_msgs[0].odometry.body_F_source_T_target.pose)); + const Eigen::Isometry3d true_body_F_target_T_target_2 = + params.body_T_haz_cam * target_2_T_target.inverse() * (params.body_T_haz_cam).inverse(); + EXPECT_PRED2(lc::MatrixEquality<2>, body_F_target_T_target_2.matrix(), true_body_F_target_T_target_2.matrix()); + } +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/depth_odometry/test/test_depth_odometry_wrapper.test b/localization/depth_odometry/test/test_depth_odometry_wrapper.test new file mode 100644 index 0000000000..c9cf302f9d --- /dev/null +++ b/localization/depth_odometry/test/test_depth_odometry_wrapper.test @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/localization/depth_odometry/test/test_image_features_with_known_correspondences_aligner_depth_odometry.cc b/localization/depth_odometry/test/test_image_features_with_known_correspondences_aligner_depth_odometry.cc new file mode 100644 index 0000000000..0fcdc9f2e3 --- /dev/null +++ b/localization/depth_odometry/test/test_image_features_with_known_correspondences_aligner_depth_odometry.cc @@ -0,0 +1,184 @@ +/* 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. + */ + +#include "test_utilities.h" // NOLINT +#include +#include +#include +#include + +#include + +namespace dd = depth_odometry; +namespace lc = localization_common; + +TEST(ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryTester, RampedPoints) { + auto params = dd::DefaultImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams(); + dd::ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry image_features_depth_odometry(params); + const auto source_depth_image_measurement = dd::ImageFeatureDepthImageMeasurement(0); + constexpr double translation_stddev = 1; + constexpr double rotation_stddev = 1; + const auto target_T_source = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const cv::Point2i offset(5, 5); + const auto target_depth_image_measurement = + dd::OffsetImageFeatureDepthImageMeasurement(0.1, source_depth_image_measurement, offset, target_T_source); + { + const auto pose_with_covariance = image_features_depth_odometry.DepthImageCallback(source_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance == boost::none); + } + const auto pose_with_covariance = image_features_depth_odometry.DepthImageCallback(target_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance != boost::none); + EXPECT_PRED2(lc::MatrixEquality<2>, pose_with_covariance->pose_with_covariance.pose.matrix(), + target_T_source.inverse().matrix()); + const auto& correspondences = pose_with_covariance->depth_correspondences; + for (int i = 0; i < correspondences.source_image_points.size(); ++i) { + EXPECT_PRED2(lc::MatrixEquality<2>, correspondences.source_image_points[i].matrix(), + (correspondences.target_image_points[i] - Eigen::Vector2d(offset.x, offset.y)).matrix()); + EXPECT_PRED2(lc::MatrixEquality<2>, target_T_source * correspondences.source_3d_points[i].matrix(), + correspondences.target_3d_points[i].matrix()); + } +} + +TEST(ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryTester, PointToPlaneRampedPoints) { + auto params = dd::DefaultImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams(); + params.aligner.use_point_to_plane_cost = true; + params.aligner.normal_search_radius = 3.0; + params.position_covariance_threshold = 10; + params.orientation_covariance_threshold = 10; + dd::ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry image_features_depth_odometry(params); + const auto source_depth_image_measurement = dd::ImageFeatureDepthImageMeasurement(0); + constexpr double translation_stddev = 1; + constexpr double rotation_stddev = 1; + const auto target_T_source = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const cv::Point2i offset(5, 5); + const auto target_depth_image_measurement = + dd::OffsetImageFeatureDepthImageMeasurement(0.1, source_depth_image_measurement, offset, target_T_source); + { + const auto pose_with_covariance = image_features_depth_odometry.DepthImageCallback(source_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance == boost::none); + } + const auto pose_with_covariance = image_features_depth_odometry.DepthImageCallback(target_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance != boost::none); + EXPECT_PRED2(lc::MatrixEquality<2>, pose_with_covariance->pose_with_covariance.pose.matrix(), + target_T_source.inverse().matrix()); + const auto& correspondences = pose_with_covariance->depth_correspondences; + for (int i = 0; i < correspondences.source_image_points.size(); ++i) { + EXPECT_PRED2(lc::MatrixEquality<2>, correspondences.source_image_points[i].matrix(), + (correspondences.target_image_points[i] - Eigen::Vector2d(offset.x, offset.y)).matrix()); + EXPECT_PRED2(lc::MatrixEquality<2>, target_T_source * correspondences.source_3d_points[i].matrix(), + correspondences.target_3d_points[i].matrix()); + } +} + +TEST(ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryTester, SymmetricPointToPlaneRampedPoints) { + auto params = dd::DefaultImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams(); + params.aligner.use_symmetric_point_to_plane_cost = true; + params.aligner.normal_search_radius = 3.0; + params.position_covariance_threshold = 10; + params.orientation_covariance_threshold = 10; + dd::ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry image_features_depth_odometry(params); + const auto source_depth_image_measurement = dd::ImageFeatureDepthImageMeasurement(0); + constexpr double translation_stddev = 1; + constexpr double rotation_stddev = 1; + const auto target_T_source = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const cv::Point2i offset(5, 5); + const auto target_depth_image_measurement = + dd::OffsetImageFeatureDepthImageMeasurement(0.1, source_depth_image_measurement, offset, target_T_source); + { + const auto pose_with_covariance = image_features_depth_odometry.DepthImageCallback(source_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance == boost::none); + } + const auto pose_with_covariance = image_features_depth_odometry.DepthImageCallback(target_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance != boost::none); + EXPECT_PRED2(lc::MatrixEquality<2>, pose_with_covariance->pose_with_covariance.pose.matrix(), + target_T_source.inverse().matrix()); + const auto& correspondences = pose_with_covariance->depth_correspondences; + for (int i = 0; i < correspondences.source_image_points.size(); ++i) { + EXPECT_PRED2(lc::MatrixEquality<2>, correspondences.source_image_points[i].matrix(), + (correspondences.target_image_points[i] - Eigen::Vector2d(offset.x, offset.y)).matrix()); + EXPECT_PRED2(lc::MatrixEquality<2>, target_T_source * correspondences.source_3d_points[i].matrix(), + correspondences.target_3d_points[i].matrix()); + } +} + +TEST(ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryTester, UmeyamaRampedPoints) { + auto params = dd::DefaultImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams(); + params.aligner.use_single_iteration_umeyama = true; + dd::ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry image_features_depth_odometry(params); + const auto source_depth_image_measurement = dd::ImageFeatureDepthImageMeasurement(0); + constexpr double translation_stddev = 1; + constexpr double rotation_stddev = 1; + const auto target_T_source = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const cv::Point2i offset(5, 5); + const auto target_depth_image_measurement = + dd::OffsetImageFeatureDepthImageMeasurement(0.1, source_depth_image_measurement, offset, target_T_source); + { + const auto pose_with_covariance = image_features_depth_odometry.DepthImageCallback(source_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance == boost::none); + } + const auto pose_with_covariance = image_features_depth_odometry.DepthImageCallback(target_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance != boost::none); + EXPECT_PRED2(lc::MatrixEquality<2>, pose_with_covariance->pose_with_covariance.pose.matrix(), + target_T_source.inverse().matrix()); + const auto& correspondences = pose_with_covariance->depth_correspondences; + for (int i = 0; i < correspondences.source_image_points.size(); ++i) { + EXPECT_PRED2(lc::MatrixEquality<2>, correspondences.source_image_points[i].matrix(), + (correspondences.target_image_points[i] - Eigen::Vector2d(offset.x, offset.y)).matrix()); + EXPECT_PRED2(lc::MatrixEquality<2>, target_T_source * correspondences.source_3d_points[i].matrix(), + correspondences.target_3d_points[i].matrix()); + } +} + +TEST(ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryTester, UmeyamaInitialGeussRampedPoints) { + auto params = dd::DefaultImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams(); + params.aligner.use_umeyama_initial_guess = true; + dd::ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry image_features_depth_odometry(params); + const auto source_depth_image_measurement = dd::ImageFeatureDepthImageMeasurement(0); + constexpr double translation_stddev = 1; + constexpr double rotation_stddev = 1; + const auto target_T_source = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const cv::Point2i offset(5, 5); + const auto target_depth_image_measurement = + dd::OffsetImageFeatureDepthImageMeasurement(0.1, source_depth_image_measurement, offset, target_T_source); + { + const auto pose_with_covariance = image_features_depth_odometry.DepthImageCallback(source_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance == boost::none); + } + const auto pose_with_covariance = image_features_depth_odometry.DepthImageCallback(target_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance != boost::none); + EXPECT_PRED2(lc::MatrixEquality<2>, pose_with_covariance->pose_with_covariance.pose.matrix(), + target_T_source.inverse().matrix()); + const auto& correspondences = pose_with_covariance->depth_correspondences; + for (int i = 0; i < correspondences.source_image_points.size(); ++i) { + EXPECT_PRED2(lc::MatrixEquality<2>, correspondences.source_image_points[i].matrix(), + (correspondences.target_image_points[i] - Eigen::Vector2d(offset.x, offset.y)).matrix()); + EXPECT_PRED2(lc::MatrixEquality<2>, target_T_source * correspondences.source_3d_points[i].matrix(), + correspondences.target_3d_points[i].matrix()); + } +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/depth_odometry/test/test_image_features_with_known_correspondences_aligner_depth_odometry.test b/localization/depth_odometry/test/test_image_features_with_known_correspondences_aligner_depth_odometry.test new file mode 100644 index 0000000000..169da15a51 --- /dev/null +++ b/localization/depth_odometry/test/test_image_features_with_known_correspondences_aligner_depth_odometry.test @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/localization/depth_odometry/test/test_point_to_plane_icp_depth_odometry.cc b/localization/depth_odometry/test/test_point_to_plane_icp_depth_odometry.cc new file mode 100644 index 0000000000..4867a6d69b --- /dev/null +++ b/localization/depth_odometry/test/test_point_to_plane_icp_depth_odometry.cc @@ -0,0 +1,158 @@ +/* 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. + */ + +#include "test_utilities.h" // NOLINT +#include +#include +#include +#include + +#include + +namespace dd = depth_odometry; +namespace lc = localization_common; + +TEST(PointToPlaneICPDepthOdometryTester, PointToPlaneCubicPoints) { + auto params = dd::DefaultPointToPlaneICPDepthOdometryParams(); + params.icp.search_radius = 1.0; + dd::PointToPlaneICPDepthOdometry icp_depth_odometry(params); + + const auto source_depth_image_measurement = dd::DefaultDepthImageMeasurement(0); + constexpr double translation_stddev = 0.01; + constexpr double rotation_stddev = 0.01; + const auto target_T_source = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_depth_image_measurement = + dd::TransformDepthImageMeasurement(source_depth_image_measurement, 0.1, target_T_source); + { + const auto pose_with_covariance = icp_depth_odometry.DepthImageCallback(source_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance == boost::none); + } + const auto pose_with_covariance = icp_depth_odometry.DepthImageCallback(target_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance != boost::none); + EXPECT_PRED2(lc::MatrixEquality<4>, pose_with_covariance->pose_with_covariance.pose.matrix(), + target_T_source.inverse().matrix()); +} + +TEST(PointToPlaneICPDepthOdometryTester, PointToPlaneDownsampledCubicPoints) { + auto params = dd::DefaultPointToPlaneICPDepthOdometryParams(); + params.icp.search_radius = 1.0; + params.downsample = true; + params.downsample_leaf_size = 0.03; + dd::PointToPlaneICPDepthOdometry icp_depth_odometry(params); + + const auto source_depth_image_measurement = dd::DefaultDepthImageMeasurement(0); + constexpr double translation_stddev = 0.01; + constexpr double rotation_stddev = 0.01; + const auto target_T_source = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_depth_image_measurement = + dd::TransformDepthImageMeasurement(source_depth_image_measurement, 0.1, target_T_source); + { + const auto pose_with_covariance = icp_depth_odometry.DepthImageCallback(source_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance == boost::none); + } + const auto pose_with_covariance = icp_depth_odometry.DepthImageCallback(target_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance != boost::none); + EXPECT_PRED2(lc::MatrixEquality<4>, pose_with_covariance->pose_with_covariance.pose.matrix(), + target_T_source.inverse().matrix()); +} + +TEST(PointToPlaneICPDepthOdometryTester, SymmetricPointToPlaneCubicPoints) { + auto params = dd::DefaultPointToPlaneICPDepthOdometryParams(); + params.icp.search_radius = 1.0; + params.icp.symmetric_objective = true; + dd::PointToPlaneICPDepthOdometry icp_depth_odometry(params); + + const auto source_depth_image_measurement = dd::DefaultDepthImageMeasurement(0); + constexpr double translation_stddev = 0.01; + constexpr double rotation_stddev = 0.01; + const auto target_T_source = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_depth_image_measurement = + dd::TransformDepthImageMeasurement(source_depth_image_measurement, 0.1, target_T_source); + { + const auto pose_with_covariance = icp_depth_odometry.DepthImageCallback(source_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance == boost::none); + } + const auto pose_with_covariance = icp_depth_odometry.DepthImageCallback(target_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance != boost::none); + EXPECT_PRED2(lc::MatrixEquality<4>, pose_with_covariance->pose_with_covariance.pose.matrix(), + target_T_source.inverse().matrix()); +} + +TEST(PointToPlaneICPDepthOdometryTester, CorrespondenceRejectorPointToPlaneCubicPoints) { + auto params = dd::DefaultPointToPlaneICPDepthOdometryParams(); + params.icp.search_radius = 1.0; + params.icp.correspondence_rejector_surface_normal = true; + dd::PointToPlaneICPDepthOdometry icp_depth_odometry(params); + + const auto source_depth_image_measurement = dd::DefaultDepthImageMeasurement(0); + constexpr double translation_stddev = 0.01; + constexpr double rotation_stddev = 0.01; + const auto target_T_source = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_depth_image_measurement = + dd::TransformDepthImageMeasurement(source_depth_image_measurement, 0.1, target_T_source); + { + const auto pose_with_covariance = icp_depth_odometry.DepthImageCallback(source_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance == boost::none); + } + const auto pose_with_covariance = icp_depth_odometry.DepthImageCallback(target_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance != boost::none); + EXPECT_PRED2(lc::MatrixEquality<4>, pose_with_covariance->pose_with_covariance.pose.matrix(), + target_T_source.inverse().matrix()); +} + +TEST(PointToPlaneICPDepthOdometryTester, PointToPlane3MeasurementsCubicPoints) { + auto params = dd::DefaultPointToPlaneICPDepthOdometryParams(); + params.icp.search_radius = 1.0; + dd::PointToPlaneICPDepthOdometry icp_depth_odometry(params); + + const auto source_depth_image_measurement = dd::DefaultDepthImageMeasurement(0); + constexpr double translation_stddev = 0.01; + constexpr double rotation_stddev = 0.01; + const auto target_T_source = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target_depth_image_measurement = + dd::TransformDepthImageMeasurement(source_depth_image_measurement, 0.1, target_T_source); + { + const auto pose_with_covariance = icp_depth_odometry.DepthImageCallback(source_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance == boost::none); + } + const auto pose_with_covariance = icp_depth_odometry.DepthImageCallback(target_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance != boost::none); + EXPECT_PRED2(lc::MatrixEquality<4>, pose_with_covariance->pose_with_covariance.pose.matrix(), + target_T_source.inverse().matrix()); + + // Add third measurement + const auto target2_T_target = + lc::AddNoiseToIsometry3d(Eigen::Isometry3d::Identity(), translation_stddev, rotation_stddev); + const auto target2_depth_image_measurement = + dd::TransformDepthImageMeasurement(target_depth_image_measurement, 0.2, target2_T_target); + const auto pose_with_covariance2 = icp_depth_odometry.DepthImageCallback(target2_depth_image_measurement); + ASSERT_TRUE(pose_with_covariance2 != boost::none); + EXPECT_PRED2(lc::MatrixEquality<4>, pose_with_covariance2->pose_with_covariance.pose.matrix(), + target2_T_target.inverse().matrix()); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/depth_odometry/test/test_point_to_plane_icp_depth_odometry.test b/localization/depth_odometry/test/test_point_to_plane_icp_depth_odometry.test new file mode 100644 index 0000000000..d71dbe2966 --- /dev/null +++ b/localization/depth_odometry/test/test_point_to_plane_icp_depth_odometry.test @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/localization/depth_odometry/test/test_utilities.cc b/localization/depth_odometry/test/test_utilities.cc new file mode 100644 index 0000000000..9e9d075cce --- /dev/null +++ b/localization/depth_odometry/test/test_utilities.cc @@ -0,0 +1,259 @@ +/* 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. + */ + +#include "test_utilities.h" // NOLINT +#include +#include +#include +#include +#include + +#include + +#include + +#include + +namespace depth_odometry { +namespace lc = localization_common; +namespace lm = localization_measurements; +namespace pc = point_cloud_common; +namespace vc = vision_common; + +lm::DepthImageMeasurement DefaultDepthImageMeasurement(const lc::Time timestamp) { + const auto cubic_points = pc::CubicPoints(); + const auto point_cloud = pc::PointCloud(cubic_points.first); + return lm::DepthImageMeasurement(cv::Mat(), point_cloud, timestamp); +} + +std::vector RampedPoints(int cols, int rows) { + std::vector points; + for (int row = 0; row < rows; ++row) { + for (int col = 0; col < cols; ++col) { + points.emplace_back(Eigen::Vector3d(col, row, col)); + } + } + return points; +} + +lm::DepthImageMeasurement ImageFeatureDepthImageMeasurement(const lc::Time timestamp, const cv::Point2i& offset) { + int num_markers_added; + const auto image = vc::MarkerImage(33, 33, num_markers_added, offset); + const int num_points = image.cols * image.rows; + const auto points = RampedPoints(image.cols, image.rows); + const auto point_cloud = pc::PointCloud(points); + return lm::DepthImageMeasurement(image, point_cloud, timestamp); +} + +lm::DepthImageMeasurement TransformDepthImageMeasurement(const lm::DepthImageMeasurement& depth_image_measurement, + const lc::Time timestamp, + const Eigen::Isometry3d& target_T_source) { + pcl::PointCloud::Ptr transformed_cloud(new pcl::PointCloud()); + pcl::transformPointCloud(*(depth_image_measurement.depth_image.unfiltered_point_cloud()), *transformed_cloud, + Eigen::Affine3d(target_T_source.matrix())); + return lm::DepthImageMeasurement(depth_image_measurement.depth_image.image(), transformed_cloud, timestamp); +} + +lm::DepthImageMeasurement OffsetImageFeatureDepthImageMeasurement( + const lc::Time timestamp, const lm::DepthImageMeasurement& depth_image_measurement, const cv::Point2i& offset, + const Eigen::Isometry3d& target_T_source) { + pcl::PointCloud::Ptr transformed_cloud(new pcl::PointCloud()); + pcl::transformPointCloud(*(depth_image_measurement.depth_image.unfiltered_point_cloud()), *transformed_cloud, + Eigen::Affine3d(target_T_source.matrix())); + int num_markers_added; + const auto offset_image = vc::MarkerImage(33, 33, num_markers_added, offset); + const int rows = offset_image.rows; + const int cols = offset_image.cols; + pcl::PointCloud::Ptr offset_and_transformed_cloud(new pcl::PointCloud()); + const int num_points = transformed_cloud->points.size(); + // Points are in row order + for (int i = 0; i < num_points; ++i) { + const int row = i / cols; + const int col = i - row * cols; + const int new_row = row - offset.y; + const int new_col = col - offset.x; + if (new_row >= rows || new_col >= cols || new_row < 0 || new_col < 0) { + pcl::PointXYZI zero_point; + zero_point.x = 0; + zero_point.y = 0; + zero_point.z = 0; + zero_point.intensity = 0; + offset_and_transformed_cloud->points.emplace_back(zero_point); + } else { + const int new_point_index = new_row * cols + new_col; + offset_and_transformed_cloud->points.emplace_back(transformed_cloud->points[new_point_index]); + } + } + return lm::DepthImageMeasurement(offset_image, offset_and_transformed_cloud, timestamp); +} + +sensor_msgs::PointCloud2ConstPtr CubicPointsMsg(const lc::Time timestamp) { + const auto cubic_points = pc::CubicPoints(); + auto point_cloud = pc::PointCloud(cubic_points.first); + // DepthImageMeasurement expects a point cloud with a width and height to correlate with an intenisty image + // TODO(rsoussan): Replace with resize when pcl version is updated + // point_cloud->resize(20, 15); + pcl::PointCloud resized_point_cloud(20, 15); + int i = 0; + for (const auto& point : point_cloud->points) { + resized_point_cloud.points[i++] = point; + } + sensor_msgs::PointCloud2 msg; + lc::TimeToHeader(timestamp, msg.header); + pcl::toROSMsg(resized_point_cloud, msg); + return sensor_msgs::PointCloud2ConstPtr(new sensor_msgs::PointCloud2(msg)); +} + +sensor_msgs::PointCloud2ConstPtr RampedPointsMsg(const lc::Time timestamp) { + const int cols = 640; + const int rows = 480; + const auto ramped_points = RampedPoints(cols, rows); + auto point_cloud = pc::PointCloud(ramped_points); + // DepthImageMeasurement expects a point cloud with a width and height to correlate with an intenisty image + // TODO(rsoussan): Replace with resize when pcl version is updated + // point_cloud->resize(cols, rows); + pcl::PointCloud resized_point_cloud(cols, rows); + int i = 0; + for (const auto& point : point_cloud->points) { + resized_point_cloud.points[i++] = point; + } + sensor_msgs::PointCloud2 msg; + lc::TimeToHeader(timestamp, msg.header); + pcl::toROSMsg(resized_point_cloud, msg); + return sensor_msgs::PointCloud2ConstPtr(new sensor_msgs::PointCloud2(msg)); +} + +sensor_msgs::PointCloud2ConstPtr TransformPointsMsg(const lc::Time timestamp, + const sensor_msgs::PointCloud2ConstPtr old_msg, + const Eigen::Isometry3d& new_T_old) { + pcl::PointCloud point_cloud; + pcl::fromROSMsg(*old_msg, point_cloud); + pcl::PointCloud transformed_cloud; + pcl::transformPointCloud(point_cloud, transformed_cloud, Eigen::Affine3d(new_T_old.matrix())); + sensor_msgs::PointCloud2 msg; + pcl::toROSMsg(transformed_cloud, msg); + lc::TimeToHeader(timestamp, msg.header); + return sensor_msgs::PointCloud2ConstPtr(new sensor_msgs::PointCloud2(msg)); +} + +sensor_msgs::PointCloud2ConstPtr OffsetAndTransformPointsMsg(const lc::Time timestamp, + const sensor_msgs::PointCloud2ConstPtr old_msg, + const cv::Point2i& offset, + const Eigen::Isometry3d& target_T_source) { + pcl::PointCloud old_point_cloud; + pcl::fromROSMsg(*old_msg, old_point_cloud); + pcl::PointCloud::Ptr transformed_cloud(new pcl::PointCloud()); + pcl::transformPointCloud(old_point_cloud, *transformed_cloud, Eigen::Affine3d(target_T_source.matrix())); + int num_markers_added; + const auto offset_image = vc::MarkerImage(33, 33, num_markers_added, offset); + const int rows = offset_image.rows; + const int cols = offset_image.cols; + pcl::PointCloud::Ptr offset_and_transformed_cloud(new pcl::PointCloud(cols, rows)); + const int num_points = transformed_cloud->points.size(); + // Points are in row order + for (int i = 0; i < num_points; ++i) { + const int row = i / cols; + const int col = i - row * cols; + const int new_row = row - offset.y; + const int new_col = col - offset.x; + if (new_row >= rows || new_col >= cols || new_row < 0 || new_col < 0) { + pcl::PointXYZI zero_point; + zero_point.x = 0; + zero_point.y = 0; + zero_point.z = 0; + zero_point.intensity = 0; + offset_and_transformed_cloud->points[i] = zero_point; + } else { + const int new_point_index = new_row * cols + new_col; + offset_and_transformed_cloud->points[i] = transformed_cloud->points[new_point_index]; + } + } + sensor_msgs::PointCloud2 msg; + pcl::toROSMsg(*offset_and_transformed_cloud, msg); + lc::TimeToHeader(timestamp, msg.header); + return sensor_msgs::PointCloud2ConstPtr(new sensor_msgs::PointCloud2(msg)); +} + +sensor_msgs::ImageConstPtr ImageMsg(const lc::Time timestamp) { + cv_bridge::CvImage msg_bridge; + msg_bridge.encoding = sensor_msgs::image_encodings::MONO8; + msg_bridge.image = cv::Mat(15, 20, CV_8UC1); + auto msg = msg_bridge.toImageMsg(); + lc::TimeToHeader(timestamp, msg->header); + return sensor_msgs::ImageConstPtr(msg); +} + +sensor_msgs::ImageConstPtr MarkerImageMsg(const lc::Time timestamp, const cv::Point2i& offset) { + cv_bridge::CvImage msg_bridge; + msg_bridge.encoding = sensor_msgs::image_encodings::MONO8; + int num_markers_added; + msg_bridge.image = vc::MarkerImage(33, 33, num_markers_added, offset); + auto msg = msg_bridge.toImageMsg(); + lc::TimeToHeader(timestamp, msg->header); + return sensor_msgs::ImageConstPtr(msg); +} + +PointToPlaneICPDepthOdometryParams DefaultPointToPlaneICPDepthOdometryParams() { + PointToPlaneICPDepthOdometryParams params; + params.icp = pc::DefaultPointToPlaneICPParams(); + params.downsample = false; + params.downsample_leaf_size = 0.02; + params.use_organized_normal_estimation = false; + params.max_depth_change_factor = 0.02; + params.normal_smoothing_size = 10.0; + params.use_normal_space_sampling = false; + params.bins_per_axis = 15; + params.num_samples = 3300; + DefaultDepthOdometryParams(params); + return params; +} + +ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams +DefaultImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams() { + ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams params; + params.aligner = pc::DefaultPointCloudWithKnownCorrespondencesAlignerParams(); + params.lk_optical_flow_feature_detector_and_matcher = vc::DefaultLKOpticalFlowFeatureDetectorAndMatcherParams(); + params.detector = "lk_optical_flow"; + params.use_clahe = false; + params.clahe_grid_length = 8; + params.clahe_clip_limit = 40; + params.min_x_distance_to_border = 0; + params.min_y_distance_to_border = 0; + params.min_num_inliers = 0; + params.refine_estimate = false; + DefaultDepthOdometryParams(params); + return params; +} + +void DefaultDepthOdometryParams(DepthOdometryParams& params) { + params.max_time_diff = 1.0; + params.position_covariance_threshold = 1.0; + params.orientation_covariance_threshold = 1.0; +} + +DepthOdometryWrapperParams DefaultDepthOdometryWrapperParams() { + DepthOdometryWrapperParams params; + params.max_image_and_point_cloud_time_diff = 0.01; + params.method = "icp"; + params.body_T_haz_cam = Eigen::Isometry3d::Identity(); + params.haz_cam_A_haz_depth = Eigen::Affine3d::Identity(); + params.icp = DefaultPointToPlaneICPDepthOdometryParams(); + params.image_features = DefaultImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams(); + return params; +} +} // namespace depth_odometry diff --git a/localization/depth_odometry/test/test_utilities.h b/localization/depth_odometry/test/test_utilities.h new file mode 100644 index 0000000000..b1be5cb7fe --- /dev/null +++ b/localization/depth_odometry/test/test_utilities.h @@ -0,0 +1,80 @@ +/* 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. + */ +#ifndef DEPTH_ODOMETRY_TEST_UTILITIES_H_ // NOLINT +#define DEPTH_ODOMETRY_TEST_UTILITIES_H_ // NOLINT + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include + +namespace depth_odometry { +localization_measurements::DepthImageMeasurement DefaultDepthImageMeasurement( + const localization_common::Time timestamp); + +localization_measurements::DepthImageMeasurement ImageFeatureDepthImageMeasurement( + const localization_common::Time timestamp, const cv::Point2i& offset = cv::Point2i(0, 0)); + +localization_measurements::DepthImageMeasurement TransformDepthImageMeasurement( + const localization_measurements::DepthImageMeasurement& depth_image_measurement, + const localization_common::Time timestamp, const Eigen::Isometry3d& target_T_source); + +localization_measurements::DepthImageMeasurement OffsetImageFeatureDepthImageMeasurement( + const localization_common::Time timestamp, + const localization_measurements::DepthImageMeasurement& depth_image_measurement, const cv::Point2i& offset, + const Eigen::Isometry3d& target_T_source); + +std::vector RampedPoints(int cols, int rows); + +sensor_msgs::PointCloud2ConstPtr RampedPointsMsg(const localization_common::Time timestamp); + +sensor_msgs::PointCloud2ConstPtr CubicPointsMsg(const localization_common::Time timestamp); + +sensor_msgs::ImageConstPtr ImageMsg(const localization_common::Time timestamp); + +sensor_msgs::ImageConstPtr MarkerImageMsg(const localization_common::Time timestamp, + const cv::Point2i& offset = cv::Point2i(0, 0)); + +sensor_msgs::PointCloud2ConstPtr TransformPointsMsg(const localization_common::Time timestamp, + const sensor_msgs::PointCloud2ConstPtr old_msg, + const Eigen::Isometry3d& transform); + +sensor_msgs::PointCloud2ConstPtr OffsetAndTransformPointsMsg(const localization_common::Time timestamp, + const sensor_msgs::PointCloud2ConstPtr old_msg, + const cv::Point2i& offset, + const Eigen::Isometry3d& target_T_source); + +PointToPlaneICPDepthOdometryParams DefaultPointToPlaneICPDepthOdometryParams(); + +ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams +DefaultImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams(); + +void DefaultDepthOdometryParams(DepthOdometryParams& params); + +DepthOdometryWrapperParams DefaultDepthOdometryWrapperParams(); +} // namespace depth_odometry +#endif // DEPTH_ODOMETRY_TEST_UTILITIES_H_ // NOLINT diff --git a/localization/graph_localizer/CMakeLists.txt b/localization/graph_localizer/CMakeLists.txt index e44fb1955a..1760e858bc 100644 --- a/localization/graph_localizer/CMakeLists.txt +++ b/localization/graph_localizer/CMakeLists.txt @@ -45,7 +45,6 @@ find_package(catkin2 REQUIRED COMPONENTS SET(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/../../cmake") find_package(OpenCV331 REQUIRED) -# include ff_nodelet to get ff_util header files since these aren't exposed elsewhere catkin_package( LIBRARIES ${PROJECT_NAME} ${GLOG_LIBRARIES} ${GTSAM_LIBRARIES} INCLUDE_DIRS include ${GLOG_INCLUDE_DIRS} ${GTSAM_INCLUDE_DIR} @@ -81,7 +80,7 @@ add_library(${PROJECT_NAME} ${cc_files} ) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES} ${catkin_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) if(CATKIN_ENABLE_TESTING) @@ -91,49 +90,63 @@ if(CATKIN_ENABLE_TESTING) test/test_rotation_factor_adder.cc ) target_link_libraries(test_rotation_factor_adder - graph_localizer gtsam ${catkin_LIBRARIES} glog + graph_localizer ${catkin_LIBRARIES} ) add_rostest_gtest(test_rotation_factor test/test_rotation_factor.test test/test_rotation_factor.cc ) target_link_libraries(test_rotation_factor - graph_localizer gtsam ${catkin_LIBRARIES} glog + graph_localizer ${catkin_LIBRARIES} ) add_rostest_gtest(test_point_to_line_factor test/test_point_to_line_factor.test test/test_point_to_line_factor.cc ) target_link_libraries(test_point_to_line_factor - graph_localizer gtsam ${catkin_LIBRARIES} glog + graph_localizer ${catkin_LIBRARIES} ) add_rostest_gtest(test_point_to_line_segment_factor test/test_point_to_line_segment_factor.test test/test_point_to_line_segment_factor.cc ) target_link_libraries(test_point_to_line_segment_factor - graph_localizer gtsam ${catkin_LIBRARIES} glog + graph_localizer ${catkin_LIBRARIES} ) add_rostest_gtest(test_point_to_plane_factor test/test_point_to_plane_factor.test test/test_point_to_plane_factor.cc ) target_link_libraries(test_point_to_plane_factor - graph_localizer gtsam ${catkin_LIBRARIES} glog + graph_localizer ${catkin_LIBRARIES} + ) + add_rostest_gtest(test_point_to_point_between_factor + test/test_point_to_point_between_factor.test + test/test_point_to_point_between_factor.cc + ) + target_link_libraries(test_point_to_point_between_factor + graph_localizer ${catkin_LIBRARIES} ) add_rostest_gtest(test_point_to_handrail_endpoint_factor test/test_point_to_handrail_endpoint_factor.test test/test_point_to_handrail_endpoint_factor.cc ) target_link_libraries(test_point_to_handrail_endpoint_factor - graph_localizer gtsam ${catkin_LIBRARIES} glog + graph_localizer ${catkin_LIBRARIES} ) add_rostest_gtest(test_silu test/test_silu.test test/test_silu.cc ) target_link_libraries(test_silu - graph_localizer gtsam ${catkin_LIBRARIES} glog + graph_localizer ${catkin_LIBRARIES} + ) + add_rostest_gtest(test_depth_odometry_factor_adder + test/test_depth_odometry_factor_adder.test + test/test_depth_odometry_factor_adder.cc + ) + target_link_libraries(test_depth_odometry_factor_adder + graph_localizer ${catkin_LIBRARIES} ) endif() diff --git a/localization/graph_localizer/include/graph_localizer/depth_odometry_factor_adder.h b/localization/graph_localizer/include/graph_localizer/depth_odometry_factor_adder.h new file mode 100644 index 0000000000..bb6d896930 --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/depth_odometry_factor_adder.h @@ -0,0 +1,43 @@ +/* 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. + */ + +#ifndef GRAPH_LOCALIZER_DEPTH_ODOMETRY_FACTOR_ADDER_H_ +#define GRAPH_LOCALIZER_DEPTH_ODOMETRY_FACTOR_ADDER_H_ + +#include +#include +#include + +#include + +namespace graph_localizer { +class DepthOdometryFactorAdder + : public graph_optimizer::FactorAdder { + using Base = + graph_optimizer::FactorAdder; + + public: + explicit DepthOdometryFactorAdder(const DepthOdometryFactorAdderParams& params); + + std::vector AddFactors( + const localization_measurements::DepthOdometryMeasurement& depth_odometry_measurement) final; +}; +} // namespace graph_localizer + +#endif // GRAPH_LOCALIZER_DEPTH_ODOMETRY_FACTOR_ADDER_H_ diff --git a/localization/graph_localizer/include/graph_localizer/depth_odometry_factor_adder_params.h b/localization/graph_localizer/include/graph_localizer/depth_odometry_factor_adder_params.h new file mode 100644 index 0000000000..95dc518a7b --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/depth_odometry_factor_adder_params.h @@ -0,0 +1,39 @@ +/* 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. + */ + +#ifndef GRAPH_LOCALIZER_DEPTH_ODOMETRY_FACTOR_ADDER_PARAMS_H_ +#define GRAPH_LOCALIZER_DEPTH_ODOMETRY_FACTOR_ADDER_PARAMS_H_ + +#include + +#include + +namespace graph_localizer { +struct DepthOdometryFactorAdderParams : public graph_optimizer::FactorAdderParams { + double noise_scale; + bool use_points_between_factor; + double position_covariance_threshold; + double orientation_covariance_threshold; + double point_to_point_error_threshold; + double pose_translation_norm_threshold; + int max_num_points_between_factors; + gtsam::Pose3 body_T_sensor; +}; +} // namespace graph_localizer + +#endif // GRAPH_LOCALIZER_DEPTH_ODOMETRY_FACTOR_ADDER_PARAMS_H_ diff --git a/localization/graph_localizer/include/graph_localizer/factor_params.h b/localization/graph_localizer/include/graph_localizer/factor_params.h index fe65e9e3b1..c432d22de9 100644 --- a/localization/graph_localizer/include/graph_localizer/factor_params.h +++ b/localization/graph_localizer/include/graph_localizer/factor_params.h @@ -18,6 +18,7 @@ #ifndef GRAPH_LOCALIZER_FACTOR_PARAMS_H_ #define GRAPH_LOCALIZER_FACTOR_PARAMS_H_ +#include #include #include #include @@ -27,6 +28,7 @@ namespace graph_localizer { struct FactorParams { + DepthOdometryFactorAdderParams depth_odometry_adder; HandrailFactorAdderParams handrail_adder; RotationFactorAdderParams rotation_adder; SmartProjectionFactorAdderParams smart_projection_adder; diff --git a/localization/graph_localizer/include/graph_localizer/graph_localizer.h b/localization/graph_localizer/include/graph_localizer/graph_localizer.h index 4989350d49..bdd73788f3 100644 --- a/localization/graph_localizer/include/graph_localizer/graph_localizer.h +++ b/localization/graph_localizer/include/graph_localizer/graph_localizer.h @@ -23,6 +23,7 @@ #include #include #include +#include #include #include #include @@ -40,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -93,6 +95,8 @@ class GraphLocalizer : public graph_optimizer::GraphOptimizer { void AddSparseMappingMeasurement( const localization_measurements::MatchedProjectionsMeasurement& matched_projections_measurement); void AddHandrailMeasurement(const localization_measurements::HandrailPointsMeasurement& handrail_points_measurement); + void AddDepthOdometryMeasurement( + const localization_measurements::DepthOdometryMeasurement& depth_odometry_measurement); bool DoPostOptimizeActions() final; const FeatureTrackIdMap& feature_tracks() const { return feature_tracker_->feature_tracks(); } @@ -153,6 +157,7 @@ class GraphLocalizer : public graph_optimizer::GraphOptimizer { // Factor Adders std::shared_ptr ar_tag_loc_factor_adder_; + std::shared_ptr depth_odometry_factor_adder_; std::shared_ptr handrail_factor_adder_; std::shared_ptr loc_factor_adder_; std::shared_ptr projection_factor_adder_; diff --git a/localization/graph_localizer/include/graph_localizer/graph_localizer_nodelet.h b/localization/graph_localizer/include/graph_localizer/graph_localizer_nodelet.h index 8ebea49c9c..588e4a42c3 100644 --- a/localization/graph_localizer/include/graph_localizer/graph_localizer_nodelet.h +++ b/localization/graph_localizer/include/graph_localizer/graph_localizer_nodelet.h @@ -18,6 +18,7 @@ #ifndef GRAPH_LOCALIZER_GRAPH_LOCALIZER_NODELET_H_ #define GRAPH_LOCALIZER_GRAPH_LOCALIZER_NODELET_H_ +#include #include #include #include @@ -98,6 +99,8 @@ class GraphLocalizerNodelet : public ff_util::FreeFlyerNodelet { void ARVisualLandmarksCallback(const ff_msgs::VisualLandmarks::ConstPtr& visual_landmarks_msg); + void DepthOdometryCallback(const ff_msgs::DepthOdometry::ConstPtr& depth_odometry_msg); + void DepthLandmarksCallback(const ff_msgs::DepthLandmarks::ConstPtr& depth_landmarks_msg); void ImuCallback(const sensor_msgs::Imu::ConstPtr& imu_msg); @@ -110,7 +113,7 @@ class GraphLocalizerNodelet : public ff_util::FreeFlyerNodelet { ros::NodeHandle private_nh_; ros::CallbackQueue private_queue_; bool localizer_enabled_ = true; - ros::Subscriber imu_sub_, of_sub_, vl_sub_, ar_sub_, dl_sub_, flight_mode_sub_; + ros::Subscriber imu_sub_, of_sub_, vl_sub_, ar_sub_, dl_sub_, depth_odometry_sub_, flight_mode_sub_; ros::Publisher state_pub_, graph_pub_, ar_tag_pose_pub_, handrail_pose_pub_, sparse_mapping_pose_pub_, reset_pub_, heartbeat_pub_; ros::ServiceServer reset_srv_, bias_srv_, bias_from_file_srv_, input_mode_srv_; @@ -128,6 +131,7 @@ class GraphLocalizerNodelet : public ff_util::FreeFlyerNodelet { localization_common::RosTimer vl_timer_ = localization_common::RosTimer("VL msg"); localization_common::RosTimer of_timer_ = localization_common::RosTimer("OF msg"); localization_common::RosTimer ar_timer_ = localization_common::RosTimer("AR msg"); + localization_common::RosTimer depth_odometry_timer_ = localization_common::RosTimer("Depth odometry msg"); localization_common::RosTimer depth_timer_ = localization_common::RosTimer("Depth msg"); localization_common::RosTimer imu_timer_ = localization_common::RosTimer("Imu msg"); localization_common::Timer callbacks_timer_ = localization_common::Timer("Callbacks"); diff --git a/localization/graph_localizer/include/graph_localizer/graph_localizer_nodelet_params.h b/localization/graph_localizer/include/graph_localizer/graph_localizer_nodelet_params.h index 8940692236..5f5f1e4756 100644 --- a/localization/graph_localizer/include/graph_localizer/graph_localizer_nodelet_params.h +++ b/localization/graph_localizer/include/graph_localizer/graph_localizer_nodelet_params.h @@ -24,6 +24,7 @@ struct GraphLocalizerNodeletParams { int max_optical_flow_buffer_size; int max_vl_buffer_size; int max_ar_buffer_size; + int max_depth_odometry_buffer_size; int max_dl_buffer_size; // Used to avoid saving ml/ar poses with too few landmark detections int loc_adder_min_num_matches; diff --git a/localization/graph_localizer/include/graph_localizer/graph_localizer_stats.h b/localization/graph_localizer/include/graph_localizer/graph_localizer_stats.h index 85b5989f27..bfa5272de5 100644 --- a/localization/graph_localizer/include/graph_localizer/graph_localizer_stats.h +++ b/localization/graph_localizer/include/graph_localizer/graph_localizer_stats.h @@ -36,6 +36,10 @@ class GraphLocalizerStats : public graph_optimizer::GraphStats { localization_common::Averager num_marginal_factors_averager_ = localization_common::Averager("Num Marginal Factors"); localization_common::Averager num_factors_averager_ = localization_common::Averager("Num Factors"); localization_common::Averager num_features_averager_ = localization_common::Averager("Num Features"); + localization_common::Averager num_depth_odometry_rel_pose_factors_averager_ = + localization_common::Averager("Num Depth Odometry Rel Pose Factors"); + localization_common::Averager num_depth_odometry_rel_point_factors_averager_ = + localization_common::Averager("Num Depth Odometry Rel Point Factors"); localization_common::Averager num_optical_flow_factors_averager_ = localization_common::Averager("Num Optical Flow Factors"); localization_common::Averager num_loc_proj_factors_averager_ = localization_common::Averager("Num Loc Proj Factors"); @@ -47,6 +51,10 @@ class GraphLocalizerStats : public graph_optimizer::GraphStats { localization_common::Averager num_vel_prior_factors_averager_ = localization_common::Averager("Num Vel Prior Factors"); // Factor Error Averagers + localization_common::Averager depth_odom_rel_pose_error_averager_ = + localization_common::Averager("Depth Odom Rel Pose Factor Error"); + localization_common::Averager depth_odom_rel_point_error_averager_ = + localization_common::Averager("Depth Odom Rel Point Factor Error"); localization_common::Averager of_error_averager_ = localization_common::Averager("OF Factor Error"); localization_common::Averager loc_proj_error_averager_ = localization_common::Averager("Loc Proj Factor Error"); localization_common::Averager loc_pose_error_averager_ = localization_common::Averager("Loc Pose Factor Error"); diff --git a/localization/graph_localizer/include/graph_localizer/graph_localizer_wrapper.h b/localization/graph_localizer/include/graph_localizer/graph_localizer_wrapper.h index 3b9660d0e0..026a551d22 100644 --- a/localization/graph_localizer/include/graph_localizer/graph_localizer_wrapper.h +++ b/localization/graph_localizer/include/graph_localizer/graph_localizer_wrapper.h @@ -18,6 +18,7 @@ #ifndef GRAPH_LOCALIZER_GRAPH_LOCALIZER_WRAPPER_H_ #define GRAPH_LOCALIZER_GRAPH_LOCALIZER_WRAPPER_H_ +#include #include #include #include @@ -80,6 +81,8 @@ class GraphLocalizerWrapper { void ARVisualLandmarksCallback(const ff_msgs::VisualLandmarks& visual_landmarks_msg); + void DepthOdometryCallback(const ff_msgs::DepthOdometry& depth_odometry_msg); + void DepthLandmarksCallback(const ff_msgs::DepthLandmarks& depth_landmarks_msg); void ImuCallback(const sensor_msgs::Imu& imu_msg); diff --git a/localization/graph_localizer/include/graph_localizer/loc_pose_factor.h b/localization/graph_localizer/include/graph_localizer/loc_pose_factor.h index 8160165576..c51d525c2b 100644 --- a/localization/graph_localizer/include/graph_localizer/loc_pose_factor.h +++ b/localization/graph_localizer/include/graph_localizer/loc_pose_factor.h @@ -19,6 +19,7 @@ #ifndef GRAPH_LOCALIZER_LOC_POSE_FACTOR_H_ #define GRAPH_LOCALIZER_LOC_POSE_FACTOR_H_ +#include #include namespace gtsam { diff --git a/localization/graph_localizer/include/graph_localizer/parameter_reader.h b/localization/graph_localizer/include/graph_localizer/parameter_reader.h index 37cca46e6b..f47e34bd2d 100644 --- a/localization/graph_localizer/include/graph_localizer/parameter_reader.h +++ b/localization/graph_localizer/include/graph_localizer/parameter_reader.h @@ -29,6 +29,7 @@ #include #include #include +#include #include #include #include @@ -40,6 +41,7 @@ namespace graph_localizer { void LoadCalibrationParams(config_reader::ConfigReader& config, CalibrationParams& params); void LoadFactorParams(config_reader::ConfigReader& config, FactorParams& params); +void LoadDepthOdometryFactorAdderParams(config_reader::ConfigReader& config, DepthOdometryFactorAdderParams& params); void LoadHandrailFactorAdderParams(config_reader::ConfigReader& config, HandrailFactorAdderParams& params); void LoadLocFactorAdderParams(config_reader::ConfigReader& config, LocFactorAdderParams& params); void LoadARTagLocFactorAdderParams(config_reader::ConfigReader& config, LocFactorAdderParams& params); diff --git a/localization/graph_localizer/include/graph_localizer/point_to_point_between_factor.h b/localization/graph_localizer/include/graph_localizer/point_to_point_between_factor.h new file mode 100644 index 0000000000..c2c6b4d618 --- /dev/null +++ b/localization/graph_localizer/include/graph_localizer/point_to_point_between_factor.h @@ -0,0 +1,92 @@ +/* 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. + */ + +#ifndef GRAPH_LOCALIZER_POINT_TO_POINT_BETWEEN_FACTOR_H_ +#define GRAPH_LOCALIZER_POINT_TO_POINT_BETWEEN_FACTOR_H_ + +#include +#include +#include + +#include + +namespace gtsam { +class PointToPointBetweenFactor : public NoiseModelFactor2 { + typedef NoiseModelFactor2 Base; + typedef PointToPointBetweenFactor This; + + public: + PointToPointBetweenFactor() {} + + PointToPointBetweenFactor(const Point3& sensor_t_point_source, const Point3& sensor_t_point_target, + const Pose3& body_T_sensor, const SharedNoiseModel& model, Key source_pose_key, + Key target_pose_key) + : Base(model, source_pose_key, target_pose_key), + sensor_t_point_source_(sensor_t_point_source), + sensor_t_point_target_(sensor_t_point_target), + body_T_sensor_(body_T_sensor) {} + + void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { + std::cout << s << "PointToPointBetweenFactor, z = "; + traits::Print(sensor_t_point_source_); + traits::Print(sensor_t_point_target_); + traits::Print(body_T_sensor_); + Base::print("", keyFormatter); + } + + bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { + const This* e = dynamic_cast(&p); + return e && Base::equals(p, tol) && + traits::Equals(this->sensor_t_point_source(), e->sensor_t_point_source(), tol) && + traits::Equals(this->sensor_t_point_target(), e->sensor_t_point_target(), tol) && + traits::Equals(this->body_T_sensor(), e->body_T_sensor(), tol); + } + + Vector evaluateError(const Pose3& world_T_body_source, const Pose3& world_T_body_target, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const override { + const auto world_t_point_source = world_T_body_source.transformFrom(body_T_sensor_ * sensor_t_point_source_, H1); + const auto world_t_point_target = world_T_body_target.transformFrom(body_T_sensor_ * sensor_t_point_target_, H2); + if (H2) *H2 = -1.0 * (*H2); + return world_t_point_source - world_t_point_target; + } + + const Point3& sensor_t_point_source() const { return sensor_t_point_source_; } + const Point3& sensor_t_point_target() const { return sensor_t_point_target_; } + const Pose3& body_T_sensor() const { return body_T_sensor_; } + + private: + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(sensor_t_point_source_); + ar& BOOST_SERIALIZATION_NVP(sensor_t_point_target_); + ar& BOOST_SERIALIZATION_NVP(body_T_sensor_); + } + + Point3 sensor_t_point_source_; + Point3 sensor_t_point_target_; + Pose3 body_T_sensor_; + + public: + GTSAM_MAKE_ALIGNED_OPERATOR_NEW +}; +} // namespace gtsam + +#endif // GRAPH_LOCALIZER_POINT_TO_POINT_BETWEEN_FACTOR_H_ diff --git a/localization/graph_localizer/src/depth_odometry_factor_adder.cc b/localization/graph_localizer/src/depth_odometry_factor_adder.cc new file mode 100644 index 0000000000..0a602e80e1 --- /dev/null +++ b/localization/graph_localizer/src/depth_odometry_factor_adder.cc @@ -0,0 +1,99 @@ +/* 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. + */ + +#include +#include +#include +#include +#include + +namespace graph_localizer { +namespace go = graph_optimizer; +namespace lc = localization_common; +namespace lm = localization_measurements; +namespace sym = gtsam::symbol_shorthand; +DepthOdometryFactorAdder::DepthOdometryFactorAdder(const DepthOdometryFactorAdderParams& params) + : DepthOdometryFactorAdder::Base(params) {} + +std::vector DepthOdometryFactorAdder::AddFactors( + const lm::DepthOdometryMeasurement& depth_odometry_measurement) { + std::vector factors_to_add_vector; + if (!lc::PoseCovarianceSane(depth_odometry_measurement.odometry.sensor_F_source_T_target.covariance, + params().position_covariance_threshold, params().orientation_covariance_threshold)) { + LogWarning("AddFactors: Sanity check failed - invalid covariance."); + return factors_to_add_vector; + } + + if (params().use_points_between_factor) { + go::FactorsToAdd points_between_factors_to_add; + const go::KeyInfo source_key_info(&sym::P, go::NodeUpdaterType::CombinedNavState, + depth_odometry_measurement.odometry.source_time); + const go::KeyInfo target_key_info(&sym::P, go::NodeUpdaterType::CombinedNavState, + depth_odometry_measurement.odometry.target_time); + for (int i = 0; i < depth_odometry_measurement.correspondences.source_3d_points.size() && + points_between_factors_to_add.size() < params().max_num_points_between_factors; + ++i) { + const Eigen::Vector3d& sensor_t_point_source = depth_odometry_measurement.correspondences.source_3d_points[i]; + const Eigen::Vector3d& sensor_t_point_target = depth_odometry_measurement.correspondences.target_3d_points[i]; + + const Eigen::Vector3d estimate_error = + sensor_t_point_source - + depth_odometry_measurement.odometry.sensor_F_source_T_target.pose * sensor_t_point_target; + const double estimate_error_norm = estimate_error.norm(); + if (estimate_error_norm > params().point_to_point_error_threshold) continue; + const auto points_between_factor_noise = + Robust(gtsam::noiseModel::Diagonal::Sigmas(estimate_error * params().noise_scale), params().huber_k); + gtsam::PointToPointBetweenFactor::shared_ptr points_between_factor(new gtsam::PointToPointBetweenFactor( + sensor_t_point_source, sensor_t_point_target, params().body_T_sensor, points_between_factor_noise, + source_key_info.UninitializedKey(), target_key_info.UninitializedKey())); + points_between_factors_to_add.push_back({{source_key_info, target_key_info}, points_between_factor}); + } + points_between_factors_to_add.SetTimestamp(depth_odometry_measurement.odometry.target_time); + LogDebug("AddFactors: Added " << points_between_factors_to_add.size() << " points between factors."); + factors_to_add_vector.emplace_back(points_between_factors_to_add); + } else { + go::FactorsToAdd relative_pose_factors_to_add; + relative_pose_factors_to_add.reserve(1); + + const double translation_norm = + depth_odometry_measurement.odometry.sensor_F_source_T_target.pose.translation().norm(); + if (translation_norm > params().pose_translation_norm_threshold) { + LogDebug("AddFactors: Ignoring pose with large translation norm. Norm: " + << translation_norm << ", threshold: " << params().pose_translation_norm_threshold); + return factors_to_add_vector; + } + + const auto relative_pose_noise = + Robust(gtsam::noiseModel::Gaussian::Covariance( + depth_odometry_measurement.odometry.body_F_source_T_target.covariance * params().noise_scale, false), + params().huber_k); + const go::KeyInfo source_key_info(&sym::P, go::NodeUpdaterType::CombinedNavState, + depth_odometry_measurement.odometry.source_time); + const go::KeyInfo target_key_info(&sym::P, go::NodeUpdaterType::CombinedNavState, + depth_odometry_measurement.odometry.target_time); + gtsam::BetweenFactor::shared_ptr relative_pose_factor(new gtsam::BetweenFactor( + source_key_info.UninitializedKey(), target_key_info.UninitializedKey(), + lc::GtPose(depth_odometry_measurement.odometry.body_F_source_T_target.pose), relative_pose_noise)); + relative_pose_factors_to_add.push_back({{source_key_info, target_key_info}, relative_pose_factor}); + relative_pose_factors_to_add.SetTimestamp(depth_odometry_measurement.odometry.target_time); + LogDebug("AddFactors: Added 1 relative pose factor."); + factors_to_add_vector.emplace_back(relative_pose_factors_to_add); + } + return factors_to_add_vector; +} +} // namespace graph_localizer diff --git a/localization/graph_localizer/src/graph_localizer.cc b/localization/graph_localizer/src/graph_localizer.cc index 68d4ea6531..18964b816b 100644 --- a/localization/graph_localizer/src/graph_localizer.cc +++ b/localization/graph_localizer/src/graph_localizer.cc @@ -23,6 +23,7 @@ #include #include #include +#include #include #include #include @@ -78,6 +79,7 @@ GraphLocalizer::GraphLocalizer(const GraphLocalizerParams& params) // Initialize Factor Adders ar_tag_loc_factor_adder_.reset( new LocFactorAdder(params_.factor.ar_tag_loc_adder, go::GraphActionCompleterType::ARTagLocProjectionFactor)); + depth_odometry_factor_adder_.reset(new DepthOdometryFactorAdder(params_.factor.depth_odometry_adder)); handrail_factor_adder_.reset(new HandrailFactorAdder(params_.factor.handrail_adder)); loc_factor_adder_.reset( new LocFactorAdder(params_.factor.loc_adder, go::GraphActionCompleterType::LocProjectionFactor)); @@ -303,11 +305,23 @@ void GraphLocalizer::AddHandrailMeasurement(const lm::HandrailPointsMeasurement& } if (params_.factor.handrail_adder.enabled) { - LogDebug("AddSparseMappingMeasurement: Adding handrail measurement."); + LogDebug("AddHandrailPointsMeasurement: Adding handrail measurement."); BufferFactors(handrail_factor_adder_->AddFactors(handrail_points_measurement)); } } +void GraphLocalizer::AddDepthOdometryMeasurement(const lm::DepthOdometryMeasurement& depth_odometry_measurement) { + if (!MeasurementRecentEnough(depth_odometry_measurement.timestamp)) { + LogDebug("AddDepthOdometryMeasurement: Measurement too old - discarding."); + return; + } + + if (params_.factor.depth_odometry_adder.enabled) { + LogDebug("AddDepthOdometryMeasurement: Adding depth odometry measurement."); + BufferFactors(depth_odometry_factor_adder_->AddFactors(depth_odometry_measurement)); + } +} + void GraphLocalizer::DoPostSlideWindowActions(const localization_common::Time oldest_allowed_time, const boost::optional& marginals) { feature_tracker_->RemoveOldFeaturePointsAndSlideWindow(oldest_allowed_time); @@ -369,6 +383,7 @@ bool GraphLocalizer::ValidGraph() const { go::NumFactors(graph_factors()) + go::NumFactors(graph_factors()) + go::NumFactors(graph_factors()) + + go::NumFactors(graph_factors()) + go::NumFactors(graph_factors()) + go::NumFactors(graph_factors()) + go::NumFactors>(graph_factors()); diff --git a/localization/graph_localizer/src/graph_localizer_nodelet.cc b/localization/graph_localizer/src/graph_localizer_nodelet.cc index 5d32234e9d..579dc75d3b 100644 --- a/localization/graph_localizer/src/graph_localizer_nodelet.cc +++ b/localization/graph_localizer/src/graph_localizer_nodelet.cc @@ -72,6 +72,9 @@ void GraphLocalizerNodelet::SubscribeAndAdvertise(ros::NodeHandle* nh) { dl_sub_ = private_nh_.subscribe(TOPIC_LOCALIZATION_HR_FEATURES, params_.max_dl_buffer_size, &GraphLocalizerNodelet::DepthLandmarksCallback, this, ros::TransportHints().tcpNoDelay()); + depth_odometry_sub_ = + private_nh_.subscribe(TOPIC_LOCALIZATION_DEPTH_ODOM, params_.max_depth_odometry_buffer_size, + &GraphLocalizerNodelet::DepthOdometryCallback, this, ros::TransportHints().tcpNoDelay()); of_sub_ = private_nh_.subscribe(TOPIC_LOCALIZATION_OF_FEATURES, params_.max_optical_flow_buffer_size, &GraphLocalizerNodelet::OpticalFlowCallback, this, ros::TransportHints().tcpNoDelay()); @@ -178,6 +181,14 @@ void GraphLocalizerNodelet::ARVisualLandmarksCallback(const ff_msgs::VisualLandm if (ValidVLMsg(*visual_landmarks_msg, params_.ar_tag_loc_adder_min_num_matches)) PublishARTagPose(); } +void GraphLocalizerNodelet::DepthOdometryCallback(const ff_msgs::DepthOdometry::ConstPtr& depth_odometry_msg) { + depth_odometry_timer_.HeaderDiff(depth_odometry_msg->header); + depth_odometry_timer_.VlogEveryN(100, 2); + + if (!localizer_enabled()) return; + graph_localizer_wrapper_.DepthOdometryCallback(*depth_odometry_msg); +} + void GraphLocalizerNodelet::DepthLandmarksCallback(const ff_msgs::DepthLandmarks::ConstPtr& depth_landmarks_msg) { depth_timer_.HeaderDiff(depth_landmarks_msg->header); depth_timer_.VlogEveryN(100, 2); diff --git a/localization/graph_localizer/src/graph_localizer_stats.cc b/localization/graph_localizer/src/graph_localizer_stats.cc index 6798f54d92..3a4b50dedf 100644 --- a/localization/graph_localizer/src/graph_localizer_stats.cc +++ b/localization/graph_localizer/src/graph_localizer_stats.cc @@ -16,6 +16,7 @@ * under the License. */ #include +#include #include #include #include @@ -38,6 +39,8 @@ GraphLocalizerStats::GraphLocalizerStats() { AddStatsAverager(num_marginal_factors_averager_); AddStatsAverager(num_factors_averager_); // AddStatsAverager(num_features_averager_); + AddStatsAverager(num_depth_odometry_rel_pose_factors_averager_); + AddStatsAverager(num_depth_odometry_rel_point_factors_averager_); AddStatsAverager(num_optical_flow_factors_averager_); AddStatsAverager(num_loc_pose_factors_averager_); AddStatsAverager(num_loc_proj_factors_averager_); @@ -46,6 +49,8 @@ GraphLocalizerStats::GraphLocalizerStats() { AddStatsAverager(num_standstill_between_factors_averager_); AddStatsAverager(num_vel_prior_factors_averager_); + AddErrorAverager(depth_odom_rel_pose_error_averager_); + AddErrorAverager(depth_odom_rel_point_error_averager_); AddErrorAverager(of_error_averager_); AddErrorAverager(loc_proj_error_averager_); AddErrorAverager(loc_pose_error_averager_); @@ -69,6 +74,8 @@ void GraphLocalizerStats::UpdateErrors(const gtsam::NonlinearFactorGraph& graph_ using ProjectionFactor = gtsam::GenericProjectionFactor; double total_error = 0; + double depth_odom_rel_pose_factor_error = 0; + double depth_odom_rel_point_factor_error = 0; double optical_flow_factor_error = 0; double loc_proj_error = 0; double loc_pose_error = 0; @@ -81,6 +88,15 @@ void GraphLocalizerStats::UpdateErrors(const gtsam::NonlinearFactorGraph& graph_ for (const auto& factor : graph_factors) { const double error = factor->error(combined_nav_state_graph_values_->values()); total_error += error; + // TODO(rsoussan): Differentiate between this and standstill between factor + const auto depth_odom_rel_pose_factor = dynamic_cast*>(factor.get()); + if (depth_odom_rel_pose_factor) { + depth_odom_rel_pose_factor_error += error; + } + const auto depth_odom_rel_point_factor = dynamic_cast(factor.get()); + if (depth_odom_rel_point_factor) { + depth_odom_rel_point_factor_error += error; + } const auto smart_factor = dynamic_cast(factor.get()); if (smart_factor) { optical_flow_factor_error += error; @@ -125,6 +141,8 @@ void GraphLocalizerStats::UpdateErrors(const gtsam::NonlinearFactorGraph& graph_ } } total_error_averager_.Update(total_error); + depth_odom_rel_pose_error_averager_.Update(depth_odom_rel_pose_factor_error); + depth_odom_rel_point_error_averager_.Update(depth_odom_rel_point_factor_error); of_error_averager_.Update(optical_flow_factor_error); loc_proj_error_averager_.Update(loc_proj_error); loc_pose_error_averager_.Update(loc_pose_error); @@ -143,6 +161,10 @@ void GraphLocalizerStats::UpdateStats(const gtsam::NonlinearFactorGraph& graph_f num_factors_averager_.Update(graph_factors.size()); num_optical_flow_factors_averager_.Update( NumSmartFactors(graph_factors, combined_nav_state_graph_values_->values(), true)); + num_depth_odometry_rel_pose_factors_averager_.Update( + go::NumFactors>(graph_factors)); + num_depth_odometry_rel_point_factors_averager_.Update( + go::NumFactors(graph_factors)); num_loc_pose_factors_averager_.Update(go::NumFactors(graph_factors)); num_loc_proj_factors_averager_.Update(go::NumFactors>(graph_factors)); num_imu_factors_averager_.Update(go::NumFactors(graph_factors)); diff --git a/localization/graph_localizer/src/graph_localizer_wrapper.cc b/localization/graph_localizer/src/graph_localizer_wrapper.cc index 27d93b2aa2..d2be170da8 100644 --- a/localization/graph_localizer/src/graph_localizer_wrapper.cc +++ b/localization/graph_localizer/src/graph_localizer_wrapper.cc @@ -200,6 +200,13 @@ void GraphLocalizerWrapper::ARVisualLandmarksCallback(const ff_msgs::VisualLandm } } +void GraphLocalizerWrapper::DepthOdometryCallback(const ff_msgs::DepthOdometry& depth_odometry_msg) { + if (graph_localizer_) { + const auto depth_odometry_measurement = lm::MakeDepthOdometryMeasurement(depth_odometry_msg); + graph_localizer_->AddDepthOdometryMeasurement(depth_odometry_measurement); + } +} + void GraphLocalizerWrapper::DepthLandmarksCallback(const ff_msgs::DepthLandmarks& depth_landmarks_msg) { feature_counts_.depth = depth_landmarks_msg.landmarks.size(); if (!ValidDepthMsg(depth_landmarks_msg)) return; diff --git a/localization/graph_localizer/src/parameter_reader.cc b/localization/graph_localizer/src/parameter_reader.cc index dd7116ef2c..a780f1f1c0 100644 --- a/localization/graph_localizer/src/parameter_reader.cc +++ b/localization/graph_localizer/src/parameter_reader.cc @@ -40,6 +40,7 @@ void LoadCalibrationParams(config_reader::ConfigReader& config, CalibrationParam void LoadFactorParams(config_reader::ConfigReader& config, FactorParams& params) { LoadHandrailFactorAdderParams(config, params.handrail_adder); + LoadDepthOdometryFactorAdderParams(config, params.depth_odometry_adder); LoadLocFactorAdderParams(config, params.loc_adder); LoadARTagLocFactorAdderParams(config, params.ar_tag_loc_adder); LoadRotationFactorAdderParams(config, params.rotation_adder); @@ -84,6 +85,21 @@ void LoadARTagLocFactorAdderParams(config_reader::ConfigReader& config, LocFacto params.cam_noise = gtsam::noiseModel::Isotropic::Sigma(2, mc::LoadDouble(config, "loc_dock_cam_noise_stddev")); } +void LoadDepthOdometryFactorAdderParams(config_reader::ConfigReader& config, DepthOdometryFactorAdderParams& params) { + params.enabled = mc::LoadBool(config, "depth_odometry_adder_enabled"); + params.huber_k = mc::LoadDouble(config, "huber_k"); + params.noise_scale = mc::LoadDouble(config, "depth_odometry_adder_noise_scale"); + params.use_points_between_factor = mc::LoadBool(config, "depth_odometry_adder_use_points_between_factor"); + params.position_covariance_threshold = mc::LoadDouble(config, "depth_odometry_adder_position_covariance_threshold"); + params.orientation_covariance_threshold = + mc::LoadDouble(config, "depth_odometry_adder_orientation_covariance_threshold"); + params.body_T_sensor = lc::LoadTransform(config, "haz_cam_transform"); + params.point_to_point_error_threshold = mc::LoadDouble(config, "depth_odometry_adder_point_to_point_error_threshold"); + params.pose_translation_norm_threshold = + mc::LoadDouble(config, "depth_odometry_adder_pose_translation_norm_threshold"); + params.max_num_points_between_factors = mc::LoadDouble(config, "depth_odometry_adder_max_num_points_between_factors"); +} + void LoadLocFactorAdderParams(config_reader::ConfigReader& config, LocFactorAdderParams& params) { params.add_pose_priors = mc::LoadBool(config, "loc_adder_add_pose_priors"); params.add_projections = mc::LoadBool(config, "loc_adder_add_projections"); @@ -251,6 +267,7 @@ void LoadGraphLocalizerNodeletParams(config_reader::ConfigReader& config, GraphL params.max_optical_flow_buffer_size = mc::LoadInt(config, "max_optical_flow_buffer_size"); params.max_vl_buffer_size = mc::LoadInt(config, "max_vl_buffer_size"); params.max_ar_buffer_size = mc::LoadInt(config, "max_ar_buffer_size"); + params.max_depth_odometry_buffer_size = mc::LoadInt(config, "max_depth_odometry_buffer_size"); params.max_dl_buffer_size = mc::LoadInt(config, "max_dl_buffer_size"); } } // namespace graph_localizer diff --git a/localization/graph_localizer/src/sanity_checker.cc b/localization/graph_localizer/src/sanity_checker.cc index de9d4b9097..b06b6ea9ad 100644 --- a/localization/graph_localizer/src/sanity_checker.cc +++ b/localization/graph_localizer/src/sanity_checker.cc @@ -18,6 +18,7 @@ #include #include +#include namespace graph_localizer { namespace lc = localization_common; @@ -36,18 +37,9 @@ bool SanityChecker::CheckPoseSanity(const gtsam::Pose3& sparse_mapping_pose, con } bool SanityChecker::CheckCovarianceSanity(const lc::CombinedNavStateCovariances& covariances) const { - bool sane = true; - if (params_.check_position_covariance) { - const double log_det_position_cov = covariances.LogDeterminantPositionCovariance(); - sane &= (log_det_position_cov <= params_.position_covariance_threshold); - } - - if (params_.check_orientation_covariance) { - const double log_det_orientation_cov = covariances.LogDeterminantOrientationCovariance(); - sane &= (log_det_orientation_cov <= params_.orientation_covariance_threshold); - } - - return sane; + return lc::PoseCovarianceSane(covariances.pose_covariance(), params_.position_covariance_threshold, + params_.orientation_covariance_threshold, params_.check_position_covariance, + params_.check_orientation_covariance); } void SanityChecker::Reset() { num_consecutive_failures_ = 0; } diff --git a/localization/graph_localizer/test/test_depth_odometry_factor_adder.cc b/localization/graph_localizer/test/test_depth_odometry_factor_adder.cc new file mode 100644 index 0000000000..a26c958b36 --- /dev/null +++ b/localization/graph_localizer/test/test_depth_odometry_factor_adder.cc @@ -0,0 +1,230 @@ +/* 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. + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace gl = graph_localizer; +namespace go = graph_optimizer; +namespace lc = localization_common; +namespace lm = localization_measurements; +namespace sym = gtsam::symbol_shorthand; + +gl::DepthOdometryFactorAdderParams DefaultParams() { + gl::DepthOdometryFactorAdderParams params; + params.enabled = true; + params.huber_k = 1.345; + params.noise_scale = 1.0; + params.use_points_between_factor = false; + params.position_covariance_threshold = 100; + params.orientation_covariance_threshold = 100; + params.point_to_point_error_threshold = 10.0; + params.pose_translation_norm_threshold = 3.0; + params.max_num_points_between_factors = 100; + params.body_T_sensor = lc::GtPose(Eigen::Isometry3d::Identity()); + return params; +} + +lm::DepthOdometryMeasurement MeasurementFromPose(const Eigen::Isometry3d& pose, const lc::Time source_time, + const lc::Time target_time) { + lm::Odometry odometry; + odometry.source_time = source_time; + odometry.target_time = target_time; + odometry.sensor_F_source_T_target.pose = pose; + odometry.sensor_F_source_T_target.covariance = Eigen::Matrix::Identity(); + odometry.body_F_source_T_target.pose = pose; + odometry.body_F_source_T_target.covariance = Eigen::Matrix::Identity(); + std::vector empty_points; + lm::DepthCorrespondences correspondences(empty_points, empty_points); + return lm::DepthOdometryMeasurement(odometry, correspondences, target_time); +} + +void AddInlierAndOutlierPoints(const int num_inliers, const int num_outliers, + const double point_to_point_error_threshold, lm::DepthOdometryMeasurement& measurement) { + for (int i = 0; i < num_inliers; ++i) { + const Eigen::Vector3d source_point = lc::RandomVector(); + const Eigen::Vector3d target_point = measurement.odometry.sensor_F_source_T_target.pose.inverse() * source_point; + measurement.correspondences.source_3d_points.emplace_back(source_point); + measurement.correspondences.target_3d_points.emplace_back(target_point); + } + for (int i = 0; i < num_outliers; ++i) { + const Eigen::Vector3d source_point = lc::RandomVector(); + const Eigen::Vector3d noise(1.1 * point_to_point_error_threshold, 0, 0); + const Eigen::Vector3d target_point = + measurement.odometry.sensor_F_source_T_target.pose.inverse() * (source_point + noise); + measurement.correspondences.source_3d_points.emplace_back(source_point); + measurement.correspondences.target_3d_points.emplace_back(target_point); + } +} + +TEST(DepthOdometryFactorAdderTester, ValidPose) { + const auto params = DefaultParams(); + gl::DepthOdometryFactorAdder factor_adder(params); + const lc::Time source_timestamp = 0; + const lc::Time target_timestamp = 0.1; + const Eigen::Isometry3d relative_pose = Eigen::Isometry3d::Identity(); + const auto measurement = MeasurementFromPose(relative_pose, source_timestamp, target_timestamp); + const auto factors_to_add_vec = factor_adder.AddFactors(measurement); + ASSERT_EQ(factors_to_add_vec.size(), 1); + EXPECT_EQ(factors_to_add_vec[0].timestamp(), target_timestamp); + ASSERT_EQ(factors_to_add_vec[0].Get().size(), 1); + const auto& factor_to_add = factors_to_add_vec[0].Get()[0]; + // Check Key Info + { + const auto& key_infos = factor_to_add.key_infos; + ASSERT_EQ(key_infos.size(), 2); + // Key Info Source + { + const auto& key_info = key_infos[0]; + EXPECT_FALSE(key_info.is_static()); + EXPECT_NEAR(key_info.timestamp(), source_timestamp, 1e-6); + EXPECT_EQ(key_info.UninitializedKey(), sym::P(0)); + EXPECT_EQ(key_info.node_updater_type(), go::NodeUpdaterType::CombinedNavState); + } + // Key Info Target + { + const auto& key_info = key_infos[1]; + EXPECT_FALSE(key_info.is_static()); + EXPECT_NEAR(key_info.timestamp(), target_timestamp, 1e-6); + EXPECT_EQ(key_info.UninitializedKey(), sym::P(0)); + EXPECT_EQ(key_info.node_updater_type(), go::NodeUpdaterType::CombinedNavState); + } + } + // Check Factors + { + const auto factor = dynamic_cast*>(factor_to_add.factor.get()); + ASSERT_TRUE(factor); + const auto& pose = factor->measured(); + EXPECT_PRED2(lc::MatrixEquality<6>, pose.matrix(), relative_pose.matrix()); + const auto& keys = factor->keys(); + const auto& key_info = factor_to_add.key_infos[0]; + EXPECT_EQ(keys[0], key_info.UninitializedKey()); + EXPECT_EQ(keys[1], key_info.UninitializedKey()); + } +} + +TEST(DepthOdometryFactorAdderTester, InvalidPose) { + const auto params = DefaultParams(); + gl::DepthOdometryFactorAdder factor_adder(params); + const lc::Time source_timestamp = 0; + const lc::Time target_timestamp = 0.1; + Eigen::Isometry3d relative_pose = Eigen::Isometry3d::Identity(); + // Ensure translation is above threshold + relative_pose.translation() += Eigen::Vector3d(1.1, 0, 0) * params.pose_translation_norm_threshold; + const auto measurement = MeasurementFromPose(relative_pose, source_timestamp, target_timestamp); + const auto factors_to_add_vec = factor_adder.AddFactors(measurement); + ASSERT_EQ(factors_to_add_vec.size(), 0); +} + +TEST(DepthOdometryFactorAdderTester, Points) { + auto params = DefaultParams(); + for (int i = 0; i < 50; ++i) { + params.use_points_between_factor = true; + gl::DepthOdometryFactorAdder factor_adder(params); + const lc::Time source_timestamp = 0; + const lc::Time target_timestamp = 0.1; + const Eigen::Isometry3d relative_pose = lc::RandomIsometry3d(); + auto measurement = MeasurementFromPose(relative_pose, source_timestamp, target_timestamp); + const int num_inliers = 2; + const int num_outliers = 2; + AddInlierAndOutlierPoints(num_inliers, num_outliers, params.point_to_point_error_threshold, measurement); + ASSERT_EQ(measurement.correspondences.source_3d_points.size(), num_inliers + num_outliers); + const auto factors_to_add_vec = factor_adder.AddFactors(measurement); + ASSERT_EQ(factors_to_add_vec.size(), 1); + EXPECT_EQ(factors_to_add_vec[0].timestamp(), target_timestamp); + ASSERT_EQ(factors_to_add_vec[0].Get().size(), num_inliers); + // Factor 0 + { + const auto& factor_to_add = factors_to_add_vec[0].Get()[0]; + // Check Key Info + { + const auto& key_infos = factor_to_add.key_infos; + ASSERT_EQ(key_infos.size(), 2); + // Key Info Source + { + const auto& key_info = key_infos[0]; + EXPECT_FALSE(key_info.is_static()); + EXPECT_NEAR(key_info.timestamp(), source_timestamp, 1e-6); + EXPECT_EQ(key_info.UninitializedKey(), sym::P(0)); + EXPECT_EQ(key_info.node_updater_type(), go::NodeUpdaterType::CombinedNavState); + } + // Key Info Target + { + const auto& key_info = key_infos[1]; + EXPECT_FALSE(key_info.is_static()); + EXPECT_NEAR(key_info.timestamp(), target_timestamp, 1e-6); + EXPECT_EQ(key_info.UninitializedKey(), sym::P(0)); + EXPECT_EQ(key_info.node_updater_type(), go::NodeUpdaterType::CombinedNavState); + } + } + // Check Factor + { + const auto factor = dynamic_cast(factor_to_add.factor.get()); + ASSERT_TRUE(factor); + const auto& source_point = factor->sensor_t_point_source(); + EXPECT_PRED2(lc::MatrixEquality<6>, source_point.matrix(), + measurement.correspondences.source_3d_points[0].matrix()); + const auto& target_point = factor->sensor_t_point_target(); + EXPECT_PRED2(lc::MatrixEquality<6>, target_point.matrix(), + measurement.correspondences.target_3d_points[0].matrix()); + const auto& body_T_sensor = factor->body_T_sensor(); + EXPECT_PRED2(lc::MatrixEquality<6>, body_T_sensor.matrix(), params.body_T_sensor.matrix()); + const auto& keys = factor->keys(); + const auto& key_info = factor_to_add.key_infos[0]; + EXPECT_EQ(keys[0], key_info.UninitializedKey()); + EXPECT_EQ(keys[1], key_info.UninitializedKey()); + } + } + // Factor 1 + { + const auto& factor_to_add = factors_to_add_vec[0].Get()[1]; + const auto factor = dynamic_cast(factor_to_add.factor.get()); + ASSERT_TRUE(factor); + const auto& source_point = factor->sensor_t_point_source(); + EXPECT_PRED2(lc::MatrixEquality<6>, source_point.matrix(), + measurement.correspondences.source_3d_points[1].matrix()); + const auto& target_point = factor->sensor_t_point_target(); + EXPECT_PRED2(lc::MatrixEquality<6>, target_point.matrix(), + measurement.correspondences.target_3d_points[1].matrix()); + const auto& body_T_sensor = factor->body_T_sensor(); + EXPECT_PRED2(lc::MatrixEquality<6>, body_T_sensor.matrix(), params.body_T_sensor.matrix()); + const auto& keys = factor->keys(); + const auto& key_info = factor_to_add.key_infos[0]; + EXPECT_EQ(keys[0], key_info.UninitializedKey()); + EXPECT_EQ(keys[1], key_info.UninitializedKey()); + } + } +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/graph_localizer/test/test_depth_odometry_factor_adder.test b/localization/graph_localizer/test/test_depth_odometry_factor_adder.test new file mode 100644 index 0000000000..7af2876610 --- /dev/null +++ b/localization/graph_localizer/test/test_depth_odometry_factor_adder.test @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/localization/graph_localizer/test/test_point_to_point_between_factor.cc b/localization/graph_localizer/test/test_point_to_point_between_factor.cc new file mode 100644 index 0000000000..a42dc9f247 --- /dev/null +++ b/localization/graph_localizer/test/test_point_to_point_between_factor.cc @@ -0,0 +1,114 @@ +/* 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. + */ + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +namespace gl = graph_localizer; +namespace lc = localization_common; +namespace sym = gtsam::symbol_shorthand; + +TEST(PointToPointBetweenFactorTester, Jacobian) { + for (int i = 0; i < 500; ++i) { + const gtsam::Point3 sensor_t_point_source = lc::RandomVector(); + const gtsam::Point3 sensor_t_point_target = lc::RandomVector(); + const gtsam::Pose3 body_T_sensor = lc::RandomPose(); + const gtsam::Pose3 world_T_body_source = lc::RandomPose(); + const gtsam::Pose3 world_T_body_target = lc::RandomPose(); + const auto noise = gtsam::noiseModel::Unit::Create(3); + const gtsam::PointToPointBetweenFactor factor(sensor_t_point_source, sensor_t_point_target, body_T_sensor, noise, + sym::P(0), sym::P(1)); + gtsam::Matrix H1; + gtsam::Matrix H2; + const auto factor_error = factor.evaluateError(world_T_body_source, world_T_body_target, H1, H2); + const auto numerical_H1 = gtsam::numericalDerivative21( + boost::function( + boost::bind(>sam::PointToPointBetweenFactor::evaluateError, factor, _1, _2, boost::none, boost::none)), + world_T_body_source, world_T_body_target, 1e-5); + EXPECT_PRED2(lc::MatrixEquality<6>, numerical_H1.matrix(), H1.matrix()); + const auto numerical_H2 = gtsam::numericalDerivative22( + boost::function( + boost::bind(>sam::PointToPointBetweenFactor::evaluateError, factor, _1, _2, boost::none, boost::none)), + world_T_body_source, world_T_body_target, 1e-5); + EXPECT_PRED2(lc::MatrixEquality<6>, numerical_H2.matrix(), H2.matrix()); + } +} + +TEST(PointToPointBetweenFactorTester, SamePointAndPoseError) { + for (int i = 0; i < 50; ++i) { + const gtsam::Point3 sensor_t_point_source = lc::RandomVector(); + const gtsam::Point3 sensor_t_point_target = sensor_t_point_source; + const gtsam::Pose3 body_T_sensor = lc::RandomPose(); + const gtsam::Pose3 world_T_body_source = lc::RandomPose(); + const gtsam::Pose3 world_T_body_target = world_T_body_source; + const auto noise = gtsam::noiseModel::Unit::Create(3); + const gtsam::PointToPointBetweenFactor factor(sensor_t_point_source, sensor_t_point_target, body_T_sensor, noise, + sym::P(0), sym::P(1)); + const auto factor_error = factor.evaluateError(world_T_body_source, world_T_body_target); + EXPECT_PRED2(lc::MatrixEquality<6>, factor_error.matrix(), Eigen::Vector3d::Zero().matrix()); + } +} + +TEST(PointToPointBetweenFactorTester, DifferentPointIdentityPosesError) { + for (int i = 0; i < 50; ++i) { + const gtsam::Point3 sensor_t_point_source = lc::RandomVector(); + const gtsam::Point3 sensor_t_point_target = lc::RandomVector(); + const gtsam::Pose3 body_T_sensor = lc::GtPose(Eigen::Isometry3d::Identity()); + const gtsam::Pose3 world_T_body_source = lc::GtPose(Eigen::Isometry3d::Identity()); + const gtsam::Pose3 world_T_body_target = world_T_body_source; + const auto noise = gtsam::noiseModel::Unit::Create(3); + const gtsam::PointToPointBetweenFactor factor(sensor_t_point_source, sensor_t_point_target, body_T_sensor, noise, + sym::P(0), sym::P(1)); + const auto factor_error = factor.evaluateError(world_T_body_source, world_T_body_target); + EXPECT_PRED2(lc::MatrixEquality<6>, factor_error.matrix(), + (sensor_t_point_source - sensor_t_point_target).matrix()); + } +} + +TEST(PointToPointBetweenFactorTester, DifferentPointSamePoseError) { + for (int i = 0; i < 50; ++i) { + const gtsam::Point3 sensor_t_point_source = lc::RandomVector(); + const gtsam::Point3 sensor_t_point_target = lc::RandomVector(); + const gtsam::Pose3 body_T_sensor = lc::RandomPose(); + const gtsam::Pose3 world_T_body_source = lc::RandomPose(); + const gtsam::Pose3 world_T_body_target = world_T_body_source; + const auto noise = gtsam::noiseModel::Unit::Create(3); + const gtsam::PointToPointBetweenFactor factor(sensor_t_point_source, sensor_t_point_target, body_T_sensor, noise, + sym::P(0), sym::P(1)); + const auto factor_error = factor.evaluateError(world_T_body_source, world_T_body_target); + EXPECT_PRED2( + lc::MatrixEquality<6>, factor_error.matrix(), + (world_T_body_source.rotation() * body_T_sensor.rotation() * (sensor_t_point_source - sensor_t_point_target)) + .matrix()); + } +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/graph_localizer/test/test_point_to_point_between_factor.test b/localization/graph_localizer/test/test_point_to_point_between_factor.test new file mode 100644 index 0000000000..375a442c11 --- /dev/null +++ b/localization/graph_localizer/test/test_point_to_point_between_factor.test @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/localization/localization_common/include/localization_common/measurement_buffer.h b/localization/localization_common/include/localization_common/measurement_buffer.h new file mode 100644 index 0000000000..fb1a43b430 --- /dev/null +++ b/localization/localization_common/include/localization_common/measurement_buffer.h @@ -0,0 +1,97 @@ +/* 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. + */ + +#ifndef LOCALIZATION_COMMON_MEASUREMENT_BUFFER_H_ +#define LOCALIZATION_COMMON_MEASUREMENT_BUFFER_H_ + +#include + +#include + +#include + +namespace localization_common { +template +class MeasurementBuffer { + public: + void Add(const Time time, const MeasurementType& measurement) { measurements_.emplace(time, measurement); } + + boost::optional Get(const localization_common::Time time) const { + const auto measurement_it = measurements_.find(time); + if (measurement_it == measurements_.end()) return boost::none; + return measurement_it->second; + } + + boost::optional GetLatest() const { + if (measurements_.empty()) return boost::none; + return measurements_.crbegin()->second; + } + + boost::optional GetNearby(const localization_common::Time time, + const double tolerance) const { + const auto upper_bound_it = measurements_.lower_bound(time); + if (upper_bound_it == measurements_.begin()) { + if (ValidTime(upper_bound_it->first, time, tolerance)) { + return upper_bound_it->second; + } else { + return boost::none; + } + } + if (upper_bound_it == measurements_.end()) { + const auto last_measurement_it = measurements_.rbegin(); + if (ValidTime(last_measurement_it->first, time, tolerance)) { + return last_measurement_it->second; + } else { + return boost::none; + } + } + + const auto lower_bound_it = std::prev(upper_bound_it); + const double lower_bound_time_diff = std::abs(lower_bound_it->first - time); + const double upper_bound_time_diff = std::abs(upper_bound_it->first - time); + const auto closest_measurement_it = + lower_bound_time_diff <= upper_bound_time_diff ? lower_bound_it : upper_bound_it; + if (ValidTime(closest_measurement_it->first, time, tolerance)) { + return closest_measurement_it->second; + } + return boost::none; + } + + void EraseUpTo(const Time oldest_allowed_time) { + const auto measurement_it = measurements_.lower_bound(oldest_allowed_time); + measurements_.erase(measurements_.begin(), measurement_it); + } + + void EraseUpToAndIncluding(const Time oldest_allowed_time) { + const auto measurement_it = measurements_.upper_bound(oldest_allowed_time); + measurements_.erase(measurements_.begin(), measurement_it); + } + + const std::map& measurements() const { return measurements_; } + + size_t size() const { return measurements_.size(); } + + private: + static bool ValidTime(const localization_common::Time time_a, const localization_common::Time time_b, + const double tolerance) { + return std::abs(time_a - time_b) <= tolerance; + } + std::map measurements_; +}; +} // namespace localization_common +#endif // LOCALIZATION_COMMON_MEASUREMENT_BUFFER_H_ diff --git a/localization/localization_common/include/localization_common/pose_with_covariance.h b/localization/localization_common/include/localization_common/pose_with_covariance.h index 67de44e0c9..177e9d20af 100644 --- a/localization/localization_common/include/localization_common/pose_with_covariance.h +++ b/localization/localization_common/include/localization_common/pose_with_covariance.h @@ -1,4 +1,3 @@ - /* Copyright (c) 2017, United States Government, as represented by the * Administrator of the National Aeronautics and Space Administration. * @@ -27,6 +26,8 @@ using PoseCovariance = Eigen::Matrix; struct PoseWithCovariance { PoseWithCovariance(const Eigen::Isometry3d& pose, const PoseCovariance& covariance) : pose(pose), covariance(covariance) {} + PoseWithCovariance() {} + Eigen::Isometry3d pose; PoseCovariance covariance; diff --git a/localization/localization_common/include/localization_common/utilities.h b/localization/localization_common/include/localization_common/utilities.h index 0ebd379b46..583ae7fe38 100644 --- a/localization/localization_common/include/localization_common/utilities.h +++ b/localization/localization_common/include/localization_common/utilities.h @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -39,6 +40,7 @@ #include #include +#include namespace localization_common { gtsam::Pose3 LoadTransform(config_reader::ConfigReader& config, const std::string& transform_config_name); @@ -70,6 +72,8 @@ Time TimeFromRosTime(const ros::Time& time); void TimeToHeader(const Time timestamp, std_msgs::Header& header); +void TimeToMsg(const Time timestamp, ros::Time& time_msg); + gtsam::Pose3 PoseFromMsg(const geometry_msgs::PoseStamped& msg); gtsam::Pose3 PoseFromMsg(const geometry_msgs::Pose& msg_pose); @@ -94,6 +98,61 @@ gtsam::Vector3 RemoveGravityFromAccelerometerMeasurement(const gtsam::Vector3& g const gtsam::Pose3& global_T_body, const gtsam::Vector3& uncorrected_accelerometer_measurement); +template +void CombinedNavStateToMsg(const CombinedNavState& combined_nav_state, LocMsgType& loc_msg); + +template +void CombinedNavStateCovariancesToMsg(const CombinedNavStateCovariances& covariances, LocMsgType& loc_msg); + +template +double LogDeterminant(const MatrixType& matrix); + +double PoseCovarianceSane(const Eigen::Matrix& pose_covariance, + const double position_covariance_threshold = 0, + const double orientation_covariance_threshold = 0, + const bool check_position_covariance = true, const bool check_orientation_covariance = true); + +Eigen::Isometry3d Isometry3d(const Eigen::Vector3d& translation, const Eigen::Matrix3d& rotation); + +double Deg2Rad(const double degrees); + +double Rad2Deg(const double radians); + +// Assumes angles in degrees, ordered as rho, phi, z +Eigen::Vector3d CylindricalToCartesian(const Eigen::Vector3d& cylindrical_coordinates); + +// Uses Euler Angles in intrinsic ypr representation in degrees +Eigen::Matrix3d RotationFromEulerAngles(const double yaw, const double pitch, const double roll); + +Eigen::Vector2d FocalLengths(const Eigen::Matrix3d& intrinsics); + +Eigen::Vector2d PrincipalPoints(const Eigen::Matrix3d& intrinsics); + +Eigen::Isometry3d FrameChangeRelativePose(const Eigen::Isometry3d& a_F_relative_pose, const Eigen::Isometry3d& b_T_a); + +Eigen::Matrix FrameChangeRelativeCovariance( + const Eigen::Matrix& a_F_relative_pose_covariance, const Eigen::Isometry3d& b_T_a); + +PoseWithCovariance FrameChangeRelativePoseWithCovariance(const PoseWithCovariance& a_F_relative_pose_with_covariance, + const Eigen::Isometry3d& b_T_a); + +PoseWithCovariance InvertPoseWithCovariance(const PoseWithCovariance& pose_with_covariance); + +template +boost::optional> LeastSquaresCovariance( + const std::vector>& cost_jacobians); + +template +std::vector Transform(const std::vector& a_T_elements, const Eigen::Isometry3d& b_T_a); + +template +std::vector Rotate(const std::vector& a_F_a_T_elements, const Eigen::Matrix3d& b_R_a); + +std::pair, std::vector> TransformPointsWithNormals( + const std::vector& points, const std::vector& normals, + const Eigen::Isometry3d& b_T_a); + +// Implementations template void CombinedNavStateToMsg(const CombinedNavState& combined_nav_state, LocMsgType& loc_msg) { PoseToMsg(combined_nav_state.pose(), loc_msg.pose); @@ -121,21 +180,46 @@ void CombinedNavStateCovariancesToMsg(const CombinedNavStateCovariances& covaria msg_conversions::VariancesToCovDiag(covariances.position_variances(), &loc_msg.cov_diag[12]); } -Eigen::Isometry3d Isometry3d(const Eigen::Vector3d& translation, const Eigen::Matrix3d& rotation); - -double Deg2Rad(const double degrees); - -double Rad2Deg(const double radians); - -// Assumes angles in degrees, ordered as rho, phi, z -Eigen::Vector3d CylindricalToCartesian(const Eigen::Vector3d& cylindrical_coordinates); +template +double LogDeterminant(const MatrixType& matrix) { + return std::log10(matrix.determinant()); +} -// Uses Euler Angles in intrinsic ypr representation in degrees -Eigen::Matrix3d RotationFromEulerAngles(const double yaw, const double pitch, const double roll); +template +boost::optional> LeastSquaresCovariance( + const std::vector>& cost_jacobians) { + const int num_costs = cost_jacobians.size(); + Eigen::MatrixXd stacked_jacobians(num_costs * CostDim, StateDim); + int row_index = 0; + for (const auto& jacobian : cost_jacobians) { + stacked_jacobians.block(row_index, 0, CostDim, StateDim) = jacobian; + row_index += CostDim; + } + const Eigen::Matrix hessian = stacked_jacobians.transpose() * stacked_jacobians; + const Eigen::FullPivLU> lu(hessian); + if (!lu.isInvertible()) return boost::none; + // Use lu inverse rather than hessian.inverse() for improved numerical stability + const Eigen::Matrix covariance = lu.inverse(); + return covariance; +} -Eigen::Vector2d FocalLengths(const Eigen::Matrix3d& intrinsics); +template +std::vector Transform(const std::vector& a_T_elements, const Eigen::Isometry3d& b_T_a) { + std::vector b_T_elements; + for (const auto& a_T_element : a_T_elements) { + b_T_elements.emplace_back(b_T_a * a_T_element); + } + return b_T_elements; +} -Eigen::Vector2d PrincipalPoints(const Eigen::Matrix3d& intrinsics); +template +std::vector Rotate(const std::vector& a_F_a_T_elements, const Eigen::Matrix3d& b_R_a) { + std::vector b_F_a_T_elements; + for (const auto& a_F_a_T_element : a_F_a_T_elements) { + b_F_a_T_elements.emplace_back(b_R_a * a_F_a_T_element); + } + return b_F_a_T_elements; +} } // namespace localization_common #endif // LOCALIZATION_COMMON_UTILITIES_H_ diff --git a/localization/localization_common/src/combined_nav_state_covariances.cc b/localization/localization_common/src/combined_nav_state_covariances.cc index 2688dc5ce6..825200306c 100644 --- a/localization/localization_common/src/combined_nav_state_covariances.cc +++ b/localization/localization_common/src/combined_nav_state_covariances.cc @@ -17,6 +17,7 @@ */ #include +#include namespace localization_common { CombinedNavStateCovariances::CombinedNavStateCovariances(const Eigen::Matrix& pose_covariance, @@ -37,17 +38,17 @@ CombinedNavStateCovariances::CombinedNavStateCovariances(const Eigen::Vector3d& } double CombinedNavStateCovariances::LogDeterminantPositionCovariance() const { - return std::log10(pose_covariance().block<3, 3>(0, 0).determinant()); + return LogDeterminant(pose_covariance().block<3, 3>(0, 0)); } double CombinedNavStateCovariances::LogDeterminantOrientationCovariance() const { - return std::log10(pose_covariance().block<3, 3>(3, 3).determinant()); + return LogDeterminant(pose_covariance().block<3, 3>(3, 3)); } Confidence CombinedNavStateCovariances::PoseConfidence(const double position_log_det_threshold, const double orientation_log_det_threshold) const { const double position_log_det = LogDeterminantPositionCovariance(); - const double orientation_log_det = std::log10(pose_covariance().block<3, 3>(3, 3).determinant()); + const double orientation_log_det = LogDeterminantOrientationCovariance(); const Confidence position_confidence = position_log_det < position_log_det_threshold ? Confidence::kGood : Confidence::kPoor; diff --git a/localization/localization_common/src/utilities.cc b/localization/localization_common/src/utilities.cc index 6cb275d65b..8518a4cb80 100644 --- a/localization/localization_common/src/utilities.cc +++ b/localization/localization_common/src/utilities.cc @@ -82,11 +82,9 @@ Time TimeFromHeader(const std_msgs::Header& header) { return GetTime(header.stam Time TimeFromRosTime(const ros::Time& time) { return GetTime(time.sec, time.nsec); } -void TimeToHeader(const Time timestamp, std_msgs::Header& header) { - ros::Time ros_time(timestamp); - header.stamp.sec = ros_time.sec; - header.stamp.nsec = ros_time.nsec; -} +void TimeToHeader(const Time timestamp, std_msgs::Header& header) { TimeToMsg(timestamp, header.stamp); } + +void TimeToMsg(const Time timestamp, ros::Time& time_msg) { time_msg = ros::Time(timestamp); } gtsam::Pose3 PoseFromMsg(const geometry_msgs::PoseStamped& msg) { return PoseFromMsg(msg.pose); } @@ -166,10 +164,11 @@ Eigen::Vector3d CylindricalToCartesian(const Eigen::Vector3d& cylindrical_coordi return cartesian_coordinates; } -Eigen::Matrix3d RotationFromEulerAngles(const double yaw, const double pitch, const double roll) { - const Eigen::AngleAxisd yaw_aa = Eigen::AngleAxisd(Deg2Rad(yaw), Eigen::Vector3d::UnitZ()); - const Eigen::AngleAxisd pitch_aa = Eigen::AngleAxisd(Deg2Rad(pitch), Eigen::Vector3d::UnitY()); - const Eigen::AngleAxisd roll_aa = Eigen::AngleAxisd(Deg2Rad(roll), Eigen::Vector3d::UnitX()); +Eigen::Matrix3d RotationFromEulerAngles(const double yaw_degrees, const double pitch_degrees, + const double roll_degrees) { + const Eigen::AngleAxisd yaw_aa = Eigen::AngleAxisd(Deg2Rad(yaw_degrees), Eigen::Vector3d::UnitZ()); + const Eigen::AngleAxisd pitch_aa = Eigen::AngleAxisd(Deg2Rad(pitch_degrees), Eigen::Vector3d::UnitY()); + const Eigen::AngleAxisd roll_aa = Eigen::AngleAxisd(Deg2Rad(roll_degrees), Eigen::Vector3d::UnitX()); // For intrinsics euler angle convention, yaw, pitch, then roll in intrinsic body frame is equivalent to // roll, pitch, then yaw in extrinsic global frame const Eigen::Matrix3d rotation(roll_aa * pitch_aa * yaw_aa); @@ -183,4 +182,60 @@ Eigen::Vector2d FocalLengths(const Eigen::Matrix3d& intrinsics) { Eigen::Vector2d PrincipalPoints(const Eigen::Matrix3d& intrinsics) { return Eigen::Vector2d(intrinsics(0, 2), intrinsics(1, 2)); } + +Eigen::Isometry3d FrameChangeRelativePose(const Eigen::Isometry3d& a_F_relative_pose, const Eigen::Isometry3d& b_T_a) { + // Consider for example a sensor odometry measurement, sensor_time_0_T_sensor_time_1. + // To find the movement of the robot body given static body_T_sensor extrinsics, + // body_time_0_T_body_time_1 = body_T_sensor * sensor_time_0_T_sensor_time_1 * sensor_T_body + return b_T_a * a_F_relative_pose * b_T_a.inverse(); +} + +Eigen::Matrix FrameChangeRelativeCovariance( + const Eigen::Matrix& a_F_relative_pose_covariance, const Eigen::Isometry3d& b_T_a) { + // TODO(rsoussan): This might be right for translation component (frame change is equivalent to single rotation), + // but might be wrong for rotation component + return gtsam::TransformCovariance(GtPose(b_T_a))(a_F_relative_pose_covariance); +} + +PoseWithCovariance FrameChangeRelativePoseWithCovariance(const PoseWithCovariance& a_F_relative_pose_with_covariance, + const Eigen::Isometry3d& b_T_a) { + PoseWithCovariance b_F_relative_pose_with_covariance; + b_F_relative_pose_with_covariance.pose = FrameChangeRelativePose(a_F_relative_pose_with_covariance.pose, b_T_a); + b_F_relative_pose_with_covariance.covariance = + FrameChangeRelativeCovariance(a_F_relative_pose_with_covariance.covariance, b_T_a); + return b_F_relative_pose_with_covariance; +} + +PoseWithCovariance InvertPoseWithCovariance(const PoseWithCovariance& pose_with_covariance) { + PoseWithCovariance inverted_pose_with_covariance; + inverted_pose_with_covariance.pose = pose_with_covariance.pose.inverse(); + // TODO(rsoussan): Do this properly!! + inverted_pose_with_covariance.covariance = pose_with_covariance.covariance; + return inverted_pose_with_covariance; +} + +double PoseCovarianceSane(const Eigen::Matrix& pose_covariance, + const double position_covariance_threshold, const double orientation_covariance_threshold, + const bool check_position_covariance, const bool check_orientation_covariance) { + bool sane = true; + if (check_position_covariance) { + const double log_det_position_cov = LogDeterminant(pose_covariance.block<3, 3>(0, 0)); + sane &= (log_det_position_cov <= position_covariance_threshold); + } + + if (check_orientation_covariance) { + const double log_det_orientation_cov = LogDeterminant(pose_covariance.block<3, 3>(3, 3)); + sane &= (log_det_orientation_cov <= orientation_covariance_threshold); + } + + return sane; +} + +std::pair, std::vector> TransformPointsWithNormals( + const std::vector& points, const std::vector& normals, + const Eigen::Isometry3d& b_T_a) { + std::vector transformed_points = Transform(points, b_T_a); + std::vector rotated_normals = Rotate(normals, b_T_a.linear()); + return std::make_pair(transformed_points, rotated_normals); +} } // namespace localization_common diff --git a/localization/localization_common/test/test_utilities.cc b/localization/localization_common/test/test_utilities.cc index b871e0465d..8a8ba52851 100644 --- a/localization/localization_common/test/test_utilities.cc +++ b/localization/localization_common/test/test_utilities.cc @@ -46,6 +46,62 @@ TEST(UtilitiesTester, EulerAngles) { } } +TEST(UtilitiesTester, FrameChangeRelativePose) { + const Eigen::Isometry3d a_F_b_T_c = lc::Isometry3d(Eigen::Vector3d(0, 0, 1), Eigen::Matrix3d::Identity()); + // Translation only shouldn't change anything + { + const Eigen::Isometry3d n_T_a = lc::Isometry3d(Eigen::Vector3d(1, 2, 3), Eigen::Matrix3d::Identity()); + const auto n_F_b_T_c = lc::FrameChangeRelativePose(a_F_b_T_c, n_T_a); + EXPECT_PRED2(lc::MatrixEquality<6>, a_F_b_T_c.matrix(), n_F_b_T_c.matrix()); + } + // Rotation should preserve zero relative rotation + { + const Eigen::Isometry3d n_T_a = lc::RandomIsometry3d(); + const auto n_F_b_T_c = lc::FrameChangeRelativePose(a_F_b_T_c, n_T_a); + EXPECT_PRED2(lc::MatrixEquality<6>, a_F_b_T_c.linear().matrix(), n_F_b_T_c.linear().matrix()); + } + // Frame change for translation component should be the same as rotating it + { + const Eigen::Isometry3d n_T_a = lc::RandomIsometry3d(); + const auto n_F_b_T_c = lc::FrameChangeRelativePose(a_F_b_T_c, n_T_a); + EXPECT_PRED2(lc::MatrixEquality<6>, (n_T_a.linear() * a_F_b_T_c.translation()).matrix(), + n_F_b_T_c.translation().matrix()); + } +} + +TEST(UtilitiesTester, FrameChangeRelativePoseWithRotation) { + const Eigen::Isometry3d a_F_b_T_c = lc::Isometry3d(Eigen::Vector3d(0, 0, 1), lc::RotationFromEulerAngles(90, 0, 0)); + // Rotation about the same axis shouldn't change relative rotation + { + const Eigen::Isometry3d n_T_a = lc::Isometry3d(Eigen::Vector3d::Zero(), lc::RotationFromEulerAngles(90, 0, 0)); + const auto n_F_b_T_c = lc::FrameChangeRelativePose(a_F_b_T_c, n_T_a); + EXPECT_PRED2(lc::MatrixEquality<6>, a_F_b_T_c.linear().matrix(), n_F_b_T_c.linear().matrix()); + } + // Rotation should be the same as n_F_b_R_c * a_F_b_R_c * n_F_b_R_c.inv() + // The rotation a_F_b_R_c can be thought of as a_R_b * b_R_c*b_R_a, + // which rotates to b's position in frame a, applies the relative rotation from b to c, then + // subtracts the rotation from b to a. + // Thus a frame change n_R_a should be applied as n_R_a* a_R_b * b_R_c*b_R_a*a_R_n + { + const Eigen::Isometry3d n_T_a = lc::RandomIsometry3d(); + const auto n_F_b_T_c = lc::FrameChangeRelativePose(a_F_b_T_c, n_T_a); + EXPECT_PRED2(lc::MatrixEquality<6>, n_T_a.linear() * a_F_b_T_c.linear() * (n_T_a.linear()).transpose(), + n_F_b_T_c.linear()); + } + // Rotation magnitude shouldn't be affected by frame change + for (int i = 0; i < 50; ++i) { + const Eigen::Isometry3d a_F_b_T_c = lc::RandomIsometry3d(); + const Eigen::Isometry3d n_T_a = lc::RandomIsometry3d(); + const auto n_F_b_T_c = lc::FrameChangeRelativePose(a_F_b_T_c, n_T_a); + // Sometimes axis can be flipped, take min angle distance so this doesn't matter + const double angle_a = + std::min(2 * M_PI - Eigen::AngleAxisd(a_F_b_T_c.linear()).angle(), Eigen::AngleAxisd(a_F_b_T_c.linear()).angle()); + const double angle_b = + std::min(2 * M_PI - Eigen::AngleAxisd(n_F_b_T_c.linear()).angle(), Eigen::AngleAxisd(n_F_b_T_c.linear()).angle()); + EXPECT_NEAR(angle_a, angle_b, 1e-6); + } +} + // Run all the tests that were declared with TEST() int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/localization/localization_measurements/CMakeLists.txt b/localization/localization_measurements/CMakeLists.txt index 648695cab9..ffa87720ff 100644 --- a/localization/localization_measurements/CMakeLists.txt +++ b/localization/localization_measurements/CMakeLists.txt @@ -27,16 +27,24 @@ find_package(catkin2 REQUIRED COMPONENTS config_reader localization_common msg_conversions + point_cloud_common ) +find_package(PCL REQUIRED COMPONENTS common features filters octree search) + +find_package(pcl_conversions REQUIRED) + +# Find OpenCV +SET(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/../../cmake") +find_package(OpenCV331 REQUIRED) # System dependencies are found with CMake's conventions find_package(Eigen3 REQUIRED) catkin_package( - LIBRARIES ${PROJECT_NAME} - INCLUDE_DIRS include - CATKIN_DEPENDS ff_msgs config_reader localization_common msg_conversions + LIBRARIES ${PROJECT_NAME} ${OpenCV_LIBRARIES} ${PCL_LIBRARIES} ${pcl_conversions_INCLUDE_DIRS} + INCLUDE_DIRS include ${PCL_INCLUDE_DIRS} + CATKIN_DEPENDS ff_msgs config_reader localization_common msg_conversions point_cloud_common ) ########### @@ -48,22 +56,19 @@ include_directories( include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} + ${pcl_conversions_INCLUDE_DIRS} ) -# create_library(TARGET ${PROJECT_NAME} -# LIBS ${catkin_LIBRARIES} ff_common gtsam localization_common -# INC ${catkin_INCLUDE_DIRS} -# DEPS ff_msgs -# ) - - # Declare C++ libraries add_library(${PROJECT_NAME} + src/depth_image.cc src/measurement_conversions.cc ) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) if(CATKIN_ENABLE_TESTING) @@ -76,6 +81,14 @@ if(CATKIN_ENABLE_TESTING) localization_measurements gtsam ) find_package(rostest REQUIRED) + add_rostest_gtest(test_depth_image + test/test_depth_image.test + test/test_depth_image.cc + ) + target_link_libraries(test_depth_image + localization_measurements gtsam + ) + find_package(rostest REQUIRED) add_rostest_gtest(test_measurement_conversions test/test_measurement_conversions.test test/test_measurement_conversions.cc @@ -102,4 +115,4 @@ install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} FILES_MATCHING PATTERN "*.h" PATTERN ".svn" EXCLUDE -) \ No newline at end of file +) diff --git a/localization/localization_measurements/include/localization_measurements/depth_correspondences.h b/localization/localization_measurements/include/localization_measurements/depth_correspondences.h new file mode 100644 index 0000000000..97a6234bd0 --- /dev/null +++ b/localization/localization_measurements/include/localization_measurements/depth_correspondences.h @@ -0,0 +1,54 @@ +/* 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. + */ +#ifndef LOCALIZATION_MEASUREMENTS_DEPTH_CORRESPONDENCES_H_ +#define LOCALIZATION_MEASUREMENTS_DEPTH_CORRESPONDENCES_H_ + +#include + +#include + +namespace localization_measurements { +struct DepthCorrespondences { + DepthCorrespondences(const std::vector& source_image_points, + const std::vector& target_image_points, + const std::vector& source_3d_points, + const std::vector& target_3d_points) + : source_image_points(source_image_points), + target_image_points(target_image_points), + source_3d_points(source_3d_points), + target_3d_points(target_3d_points), + valid_image_points(true), + valid_3d_points(true) {} + + DepthCorrespondences(const std::vector& source_3d_points, + const std::vector& target_3d_points) + : source_3d_points(source_3d_points), + target_3d_points(target_3d_points), + valid_image_points(false), + valid_3d_points(true) {} + + bool valid_image_points; + bool valid_3d_points; + std::vector source_image_points; + std::vector target_image_points; + std::vector source_3d_points; + std::vector target_3d_points; +}; +} // namespace localization_measurements + +#endif // LOCALIZATION_MEASUREMENTS_DEPTH_CORRESPONDENCES_H_ diff --git a/localization/localization_measurements/include/localization_measurements/depth_image.h b/localization/localization_measurements/include/localization_measurements/depth_image.h new file mode 100644 index 0000000000..2923903899 --- /dev/null +++ b/localization/localization_measurements/include/localization_measurements/depth_image.h @@ -0,0 +1,51 @@ +/* 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. + */ + +#ifndef LOCALIZATION_MEASUREMENTS_DEPTH_IMAGE_H_ +#define LOCALIZATION_MEASUREMENTS_DEPTH_IMAGE_H_ + +#include + +#include + +#include + +#include +#include + +namespace localization_measurements { +class DepthImage { + public: + DepthImage(const cv::Mat& image, const pcl::PointCloud::Ptr point_cloud); + // Point lookups using depth image col and rows need to use the unfiltered point cloud since the indices + // of the unfiltered cloud correlate to the image space indices. + boost::optional UnfilteredPoint3D(const int col, const int row) const; + boost::optional UnfilteredPoint3D(const double col, const double row) const; + boost::optional InterpolatePoint3D(const double col, const double row) const; + const cv::Mat& image() const { return image_; } + const pcl::PointCloud::Ptr unfiltered_point_cloud() const { return unfiltered_point_cloud_; } + + private: + static bool ValidPoint(const boost::optional point); + + cv::Mat image_; + pcl::PointCloud::Ptr unfiltered_point_cloud_; +}; +} // namespace localization_measurements + +#endif // LOCALIZATION_MEASUREMENTS_DEPTH_IMAGE_H_ diff --git a/localization/localization_measurements/include/localization_measurements/depth_image_measurement.h b/localization/localization_measurements/include/localization_measurements/depth_image_measurement.h new file mode 100644 index 0000000000..f9dd102816 --- /dev/null +++ b/localization/localization_measurements/include/localization_measurements/depth_image_measurement.h @@ -0,0 +1,40 @@ +/* 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. + */ + +#ifndef LOCALIZATION_MEASUREMENTS_DEPTH_IMAGE_MEASUREMENT_H_ +#define LOCALIZATION_MEASUREMENTS_DEPTH_IMAGE_MEASUREMENT_H_ + +#include +#include +#include + +#include + +#include +#include + +namespace localization_measurements { +struct DepthImageMeasurement : public Measurement { + DepthImageMeasurement(const cv::Mat& image, const pcl::PointCloud::Ptr point_cloud, + const localization_common::Time timestamp) + : Measurement(timestamp), depth_image(image, point_cloud) {} + DepthImage depth_image; +}; +} // namespace localization_measurements + +#endif // LOCALIZATION_MEASUREMENTS_DEPTH_IMAGE_MEASUREMENT_H_ diff --git a/localization/localization_measurements/include/localization_measurements/depth_odometry_measurement.h b/localization/localization_measurements/include/localization_measurements/depth_odometry_measurement.h new file mode 100644 index 0000000000..263deba475 --- /dev/null +++ b/localization/localization_measurements/include/localization_measurements/depth_odometry_measurement.h @@ -0,0 +1,37 @@ +/* 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. + */ + +#ifndef LOCALIZATION_MEASUREMENTS_DEPTH_ODOMETRY_MEASUREMENT_H_ +#define LOCALIZATION_MEASUREMENTS_DEPTH_ODOMETRY_MEASUREMENT_H_ + +#include +#include +#include +#include + +namespace localization_measurements { +struct DepthOdometryMeasurement : public Measurement { + DepthOdometryMeasurement(const Odometry& odometry, const DepthCorrespondences& correspondences, + const localization_common::Time timestamp) + : Measurement(timestamp), odometry(odometry), correspondences(correspondences) {} + Odometry odometry; + DepthCorrespondences correspondences; +}; +} // namespace localization_measurements + +#endif // LOCALIZATION_MEASUREMENTS_DEPTH_ODOMETRY_MEASUREMENT_H_ diff --git a/localization/localization_measurements/include/localization_measurements/image_measurement.h b/localization/localization_measurements/include/localization_measurements/image_measurement.h new file mode 100644 index 0000000000..dbbe072c77 --- /dev/null +++ b/localization/localization_measurements/include/localization_measurements/image_measurement.h @@ -0,0 +1,35 @@ +/* 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. + */ + +#ifndef LOCALIZATION_MEASUREMENTS_IMAGE_MEASUREMENT_H_ +#define LOCALIZATION_MEASUREMENTS_IMAGE_MEASUREMENT_H_ + +#include +#include + +#include + +namespace localization_measurements { +struct ImageMeasurement : public Measurement { + ImageMeasurement(const cv::Mat& image, const localization_common::Time timestamp) + : Measurement(timestamp), image(image) {} + cv::Mat image; +}; +} // namespace localization_measurements + +#endif // LOCALIZATION_MEASUREMENTS_IMAGE_MEASUREMENT_H_ diff --git a/localization/localization_measurements/include/localization_measurements/measurement_conversions.h b/localization/localization_measurements/include/localization_measurements/measurement_conversions.h index 99fe0c5f8e..f37c7e313a 100644 --- a/localization/localization_measurements/include/localization_measurements/measurement_conversions.h +++ b/localization/localization_measurements/include/localization_measurements/measurement_conversions.h @@ -19,22 +19,37 @@ #ifndef LOCALIZATION_MEASUREMENTS_MEASUREMENT_CONVERSIONS_H_ #define LOCALIZATION_MEASUREMENTS_MEASUREMENT_CONVERSIONS_H_ +#include #include #include #include #include #include +#include +#include +#include #include #include #include +#include #include #include #include +#include #include #include +#include +#include +#include + +#include +#include + +#include #include +#include namespace localization_measurements { MatchedProjectionsMeasurement MakeMatchedProjectionsMeasurement(const ff_msgs::VisualLandmarks& visual_landmarks); @@ -54,6 +69,34 @@ MatchedProjectionsMeasurement FrameChangeMatchedProjectionsMeasurement( FeaturePointsMeasurement MakeFeaturePointsMeasurement(const ff_msgs::Feature2dArray& optical_flow_tracks); FanSpeedMode ConvertFanSpeedMode(const uint8_t speed); + +boost::optional MakeImageMeasurement(const sensor_msgs::ImageConstPtr& image_msg, + const std::string& encoding); + +PointCloudMeasurement MakePointCloudMeasurement(const sensor_msgs::PointCloud2ConstPtr& point_cloud_msg); + +boost::optional MakeDepthImageMeasurement( + const sensor_msgs::PointCloud2ConstPtr& depth_cloud_msg, const sensor_msgs::ImageConstPtr& image_msg, + const Eigen::Affine3d image_A_depth_cam = Eigen::Affine3d::Identity()); + +// TODO(rsoussan): Move this somewhere else? +template +sensor_msgs::PointCloud2 MakePointCloudMsg(const pcl::PointCloud& cloud, + const localization_common::Time timestamp, const std::string frame) { + sensor_msgs::PointCloud2 cloud_msg; + pcl::toROSMsg(cloud, cloud_msg); + localization_common::TimeToHeader(timestamp, cloud_msg.header); + cloud_msg.header.frame_id = frame; + return cloud_msg; +} + +localization_common::PoseWithCovariance MakePoseWithCovariance(const geometry_msgs::PoseWithCovariance& msg); + +Odometry MakeOdometry(const ff_msgs::Odometry& msg); + +DepthCorrespondences MakeDepthCorrespondences(const std::vector& msgs); + +DepthOdometryMeasurement MakeDepthOdometryMeasurement(const ff_msgs::DepthOdometry& depth_odometry_msg); } // namespace localization_measurements #endif // LOCALIZATION_MEASUREMENTS_MEASUREMENT_CONVERSIONS_H_ diff --git a/localization/localization_measurements/include/localization_measurements/odometry.h b/localization/localization_measurements/include/localization_measurements/odometry.h new file mode 100644 index 0000000000..4ff9daf481 --- /dev/null +++ b/localization/localization_measurements/include/localization_measurements/odometry.h @@ -0,0 +1,36 @@ +/* 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. + */ + +#ifndef LOCALIZATION_MEASUREMENTS_ODOMETRY_H_ +#define LOCALIZATION_MEASUREMENTS_ODOMETRY_H_ + +#include +#include +#include +#include + +namespace localization_measurements { +struct Odometry { + localization_common::Time source_time; + localization_common::Time target_time; + localization_common::PoseWithCovariance sensor_F_source_T_target; + localization_common::PoseWithCovariance body_F_source_T_target; +}; +} // namespace localization_measurements + +#endif // LOCALIZATION_MEASUREMENTS_ODOMETRY_H_ diff --git a/localization/localization_measurements/include/localization_measurements/point_cloud_measurement.h b/localization/localization_measurements/include/localization_measurements/point_cloud_measurement.h new file mode 100644 index 0000000000..f52b47bcb0 --- /dev/null +++ b/localization/localization_measurements/include/localization_measurements/point_cloud_measurement.h @@ -0,0 +1,37 @@ +/* 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. + */ + +#ifndef LOCALIZATION_MEASUREMENTS_POINT_CLOUD_MEASUREMENT_H_ +#define LOCALIZATION_MEASUREMENTS_POINT_CLOUD_MEASUREMENT_H_ + +#include +#include + +#include +#include + +namespace localization_measurements { +struct PointCloudMeasurement : public Measurement { + PointCloudMeasurement(const localization_common::Time timestamp, + const pcl::PointCloud::Ptr point_cloud) + : Measurement(timestamp), point_cloud(point_cloud) {} + pcl::PointCloud::Ptr point_cloud; +}; +} // namespace localization_measurements + +#endif // LOCALIZATION_MEASUREMENTS_POINT_CLOUD_MEASUREMENT_H_ diff --git a/localization/localization_measurements/package.xml b/localization/localization_measurements/package.xml index 27acc9a3a3..9a786c7585 100644 --- a/localization/localization_measurements/package.xml +++ b/localization/localization_measurements/package.xml @@ -18,8 +18,12 @@ config_reader localization_common msg_conversions + pcl_conversions + point_cloud_common ff_msgs config_reader localization_common msg_conversions + pcl_conversions + point_cloud_common diff --git a/localization/localization_measurements/src/depth_image.cc b/localization/localization_measurements/src/depth_image.cc new file mode 100644 index 0000000000..62a9d3c319 --- /dev/null +++ b/localization/localization_measurements/src/depth_image.cc @@ -0,0 +1,55 @@ +/* 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. + */ + +#include +#include + +namespace localization_measurements { +namespace pc = point_cloud_common; +DepthImage::DepthImage(const cv::Mat& image, const pcl::PointCloud::Ptr point_cloud) + : image_(image), unfiltered_point_cloud_(point_cloud) {} + +boost::optional DepthImage::UnfilteredPoint3D(const int col, const int row) const { + if (col >= image_.cols || col < 0 || row >= image_.rows || row < 0) return boost::none; + return unfiltered_point_cloud_->points[image_.cols * row + col]; +} +boost::optional DepthImage::UnfilteredPoint3D(const double col, const double row) const { + return UnfilteredPoint3D(static_cast(std::round(col)), static_cast(std::round(row))); +} +boost::optional DepthImage::InterpolatePoint3D(const double col, const double row) const { + double col_alpha = col - std::floor(col); + double row_alpha = row - std::floor(row); + const auto floor_col_floor_row_point = UnfilteredPoint3D(std::floor(col), std::floor(row)); + const auto ceil_col_floor_row_point = UnfilteredPoint3D(std::ceil(col), std::floor(row)); + const auto floor_col_ceil_row_point = UnfilteredPoint3D(std::floor(col), std::ceil(row)); + const auto ceil_col_ceil_row_point = UnfilteredPoint3D(std::ceil(col), std::ceil(row)); + if (!ValidPoint(floor_col_floor_row_point) || !ValidPoint(ceil_col_floor_row_point) || + !ValidPoint(floor_col_ceil_row_point) || !ValidPoint(ceil_col_ceil_row_point)) + return boost::none; + // Interpolate column points for each row option using col weights, then interpolate these results using row weights + const auto col_interpolated_floor_row = + pc::Interpolate(*floor_col_floor_row_point, *ceil_col_floor_row_point, col_alpha); + const auto col_interpolated_ceil_row = + pc::Interpolate(*floor_col_ceil_row_point, *ceil_col_ceil_row_point, col_alpha); + return pc::Interpolate(col_interpolated_floor_row, col_interpolated_ceil_row, row_alpha); +} + +bool DepthImage::ValidPoint(const boost::optional point) { + return point && pc::ValidPoint(*point); +} +} // namespace localization_measurements diff --git a/localization/localization_measurements/src/measurement_conversions.cc b/localization/localization_measurements/src/measurement_conversions.cc index 4c557aa1ea..106bd8ae94 100644 --- a/localization/localization_measurements/src/measurement_conversions.cc +++ b/localization/localization_measurements/src/measurement_conversions.cc @@ -16,12 +16,15 @@ * under the License. */ +#include #include #include #include #include +#include + #include namespace localization_measurements { @@ -136,4 +139,123 @@ FanSpeedMode ConvertFanSpeedMode(const uint8_t speed) { // Shouldn't get here return FanSpeedMode::kOff; } + +boost::optional MakeImageMeasurement(const sensor_msgs::ImageConstPtr& image_msg, + const std::string& encoding) { + cv_bridge::CvImagePtr cv_image; + try { + cv_image = cv_bridge::toCvCopy(image_msg, encoding); + } catch (cv_bridge::Exception& e) { + LogError("cv_bridge exception: " << e.what()); + return boost::none; + } + const auto timestamp = lc::TimeFromHeader(image_msg->header); + return ImageMeasurement(cv_image->image, timestamp); +} + +PointCloudMeasurement MakePointCloudMeasurement(const sensor_msgs::PointCloud2ConstPtr& point_cloud_msg) { + const lc::Time timestamp = lc::TimeFromHeader(point_cloud_msg->header); + pcl::PointCloud::Ptr point_cloud(new pcl::PointCloud()); + pcl::fromROSMsg(*point_cloud_msg, *point_cloud); + return PointCloudMeasurement(timestamp, point_cloud); +} + +boost::optional MakeDepthImageMeasurement( + const sensor_msgs::PointCloud2ConstPtr& depth_cloud_msg, const sensor_msgs::ImageConstPtr& image_msg, + const Eigen::Affine3d image_A_depth_cam) { + const auto timestamp = lc::TimeFromHeader(image_msg->header); + // TODO(rsoussan): Unify image and point cloud conversion with other functions + cv_bridge::CvImagePtr cv_image; + try { + cv_image = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::MONO8); + } catch (cv_bridge::Exception& e) { + LogError("cv_bridge exception: " << e.what()); + return boost::none; + } + const auto& intensities = cv_image->image; + + pcl::PointCloud::Ptr depth_cloud(new pcl::PointCloud()); + pcl::fromROSMsg(*depth_cloud_msg, *depth_cloud); + + if (static_cast(intensities.cols) != static_cast(depth_cloud->width) || + static_cast(intensities.rows) != static_cast(depth_cloud->height)) { + LogError("MakeDepthImageMeasurement: Image and Point Cloud dimensions do not match."); + return boost::none; + } + + pcl::PointCloud::Ptr depth_cloud_with_intensities(new pcl::PointCloud()); + depth_cloud_with_intensities->width = intensities.cols; + depth_cloud_with_intensities->height = intensities.rows; + depth_cloud_with_intensities->points.resize(depth_cloud_with_intensities->width * + depth_cloud_with_intensities->height); + + int index = 0; + for (int row = 0; row < intensities.rows; ++row) { + for (int col = 0; col < intensities.cols; ++col) { + const auto& point = depth_cloud->points[index]; + const Eigen::Vector3d depth_cam_t_point(point.x, point.y, point.z); + const Eigen::Vector3d image_t_point = image_A_depth_cam * depth_cam_t_point; + depth_cloud_with_intensities->points[index].x = image_t_point.x(); + depth_cloud_with_intensities->points[index].y = image_t_point.y(); + depth_cloud_with_intensities->points[index].z = image_t_point.z(); + depth_cloud_with_intensities->points[index].intensity = static_cast(intensities.at(row, col)); + ++index; + } + } + return DepthImageMeasurement(intensities, depth_cloud_with_intensities, timestamp); +} + +lc::PoseWithCovariance MakePoseWithCovariance(const geometry_msgs::PoseWithCovariance& msg) { + const Eigen::Isometry3d pose = lc::EigenPose(lc::PoseFromMsg(msg.pose)); + lc::PoseCovariance covariance; + for (int i = 0; i < 36; ++i) { + const int row = i / 6; + const int col = i - row * 6; + covariance(col, row) = msg.covariance[i]; + } + return lc::PoseWithCovariance(pose, covariance); +} + +Odometry MakeOdometry(const ff_msgs::Odometry& msg) { + Odometry odometry; + odometry.source_time = lc::TimeFromRosTime(msg.source_time); + odometry.target_time = lc::TimeFromRosTime(msg.target_time); + odometry.sensor_F_source_T_target = MakePoseWithCovariance(msg.sensor_F_source_T_target); + odometry.body_F_source_T_target = MakePoseWithCovariance(msg.body_F_source_T_target); + return odometry; +} + +DepthCorrespondences MakeDepthCorrespondences(const ff_msgs::DepthOdometry& msg) { + std::vector source_image_points; + std::vector target_image_points; + std::vector source_3d_points; + std::vector target_3d_points; + for (const auto& correspondence : msg.correspondences) { + if (msg.valid_image_points) { + const Eigen::Vector2d source_image_point = + mc::Vector2dFromMsg(correspondence.source_image_point); + const Eigen::Vector2d target_image_point = + mc::Vector2dFromMsg(correspondence.target_image_point); + source_image_points.emplace_back(source_image_point); + target_image_points.emplace_back(target_image_point); + } + const Eigen::Vector3d source_3d_point = + mc::VectorFromMsg(correspondence.source_3d_point); + const Eigen::Vector3d target_3d_point = + mc::VectorFromMsg(correspondence.target_3d_point); + source_3d_points.emplace_back(source_3d_point); + target_3d_points.emplace_back(target_3d_point); + } + + if (msg.valid_image_points) + return DepthCorrespondences(source_image_points, target_image_points, source_3d_points, target_3d_points); + return DepthCorrespondences(source_3d_points, target_3d_points); +} + +DepthOdometryMeasurement MakeDepthOdometryMeasurement(const ff_msgs::DepthOdometry& depth_odometry_msg) { + Odometry odometry = MakeOdometry(depth_odometry_msg.odometry); + DepthCorrespondences correspondences = MakeDepthCorrespondences(depth_odometry_msg); + const lc::Time timestamp = lc::TimeFromHeader(depth_odometry_msg.header); + return DepthOdometryMeasurement(odometry, correspondences, timestamp); +} } // namespace localization_measurements diff --git a/localization/localization_measurements/test/test_depth_image.cc b/localization/localization_measurements/test/test_depth_image.cc new file mode 100644 index 0000000000..74ae9119e7 --- /dev/null +++ b/localization/localization_measurements/test/test_depth_image.cc @@ -0,0 +1,157 @@ +/* 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. + */ + +#include +#include + +#include + +namespace lm = localization_measurements; + +lm::DepthImage RampedDepthImage() { + constexpr int num_rows = 10; + constexpr int num_cols = 10; + cv::Mat mat(cv::Mat::ones(num_rows, num_cols, CV_8UC1)); + pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); + for (int row = 0; row < num_rows; ++row) { + for (int col = 0; col < num_cols; ++col) { + pcl::PointXYZI point; + point.x = col; + point.y = row; + point.z = row * col; + point.intensity = row * col; + cloud->points.emplace_back(point); + } + } + return lm::DepthImage(mat, cloud); +} + +TEST(DepthImageTester, InvalidPoints) { + const auto depth_image = RampedDepthImage(); + { + const auto p = depth_image.UnfilteredPoint3D(-1, 1); + ASSERT_TRUE(p == boost::none); + } + { + const auto p = depth_image.UnfilteredPoint3D(1, -1); + ASSERT_TRUE(p == boost::none); + } + { + const auto p = depth_image.UnfilteredPoint3D(100, 1); + ASSERT_TRUE(p == boost::none); + } + { + const auto p = depth_image.UnfilteredPoint3D(1, 100); + ASSERT_TRUE(p == boost::none); + } + { + auto invalid_cloud = depth_image.unfiltered_point_cloud()->makeShared(); + pcl::PointXYZI invalid_point; + invalid_point.x = 0; + invalid_point.y = 0; + invalid_point.z = 0; + invalid_cloud->points[0] = invalid_point; + const lm::DepthImage invalid_depth_image(depth_image.image(), invalid_cloud); + { + const auto p = invalid_depth_image.InterpolatePoint3D(0.5, 0); + ASSERT_TRUE(p == boost::none); + } + { + const auto p = invalid_depth_image.InterpolatePoint3D(0.5, 0.5); + ASSERT_TRUE(p == boost::none); + } + { + const auto p = invalid_depth_image.InterpolatePoint3D(0, 0.5); + ASSERT_TRUE(p == boost::none); + } + { + const auto p = invalid_depth_image.InterpolatePoint3D(1.5, 1.5); + ASSERT_FALSE(p == boost::none); + } + } +} + +TEST(DepthImageTester, AccessSamePointUnfilteredAndInterpolated) { + const auto depth_image = RampedDepthImage(); + { + const auto p = depth_image.UnfilteredPoint3D(5, 5); + ASSERT_TRUE(p != boost::none); + EXPECT_NEAR(p->x, 5, 1e-6); + EXPECT_NEAR(p->y, 5, 1e-6); + EXPECT_NEAR(p->z, 25, 1e-6); + EXPECT_NEAR(p->intensity, 25, 1e-6); + } + { + const auto p = depth_image.UnfilteredPoint3D(5.1, 5.1); + ASSERT_TRUE(p != boost::none); + EXPECT_NEAR(p->x, 5, 1e-6); + EXPECT_NEAR(p->y, 5, 1e-6); + EXPECT_NEAR(p->z, 25, 1e-6); + EXPECT_NEAR(p->intensity, 25, 1e-6); + } + { + const auto p = depth_image.InterpolatePoint3D(5, 5); + ASSERT_TRUE(p != boost::none); + EXPECT_NEAR(p->x, 5, 1e-6); + EXPECT_NEAR(p->y, 5, 1e-6); + EXPECT_NEAR(p->z, 25, 1e-6); + EXPECT_NEAR(p->intensity, 25, 1e-6); + } + { + const auto p = depth_image.InterpolatePoint3D(5.1, 5); + ASSERT_TRUE(p != boost::none); + EXPECT_NEAR(p->x, 5.1, 1e-6); + EXPECT_NEAR(p->y, 5, 1e-6); + EXPECT_NEAR(p->z, 5 * 5.1, 1e-6); + EXPECT_NEAR(p->intensity, 5 * 5.1, 1e-6); + } + { + const auto p = depth_image.InterpolatePoint3D(5, 5.1); + ASSERT_TRUE(p != boost::none); + EXPECT_NEAR(p->x, 5, 1e-6); + EXPECT_NEAR(p->y, 5.1, 1e-6); + EXPECT_NEAR(p->z, 5 * 5.1, 1e-6); + EXPECT_NEAR(p->intensity, 5 * 5.1, 1e-6); + } + { + const auto p = depth_image.InterpolatePoint3D(5.5, 5.5); + ASSERT_TRUE(p != boost::none); + const auto p_55 = depth_image.UnfilteredPoint3D(5, 5); + const auto p_56 = depth_image.UnfilteredPoint3D(5, 6); + const auto p_65 = depth_image.UnfilteredPoint3D(6, 5); + const auto p_66 = depth_image.UnfilteredPoint3D(6, 6); + ASSERT_TRUE(p_55 != boost::none); + ASSERT_TRUE(p_56 != boost::none); + ASSERT_TRUE(p_65 != boost::none); + ASSERT_TRUE(p_66 != boost::none); + const double avg_x = (p_55->x + p_56->x + p_65->x + p_66->x) / 4.0; + EXPECT_NEAR(p->x, avg_x, 1e-6); + const double avg_y = (p_55->y + p_56->y + p_65->y + p_66->y) / 4.0; + EXPECT_NEAR(p->y, avg_y, 1e-6); + const double avg_z = (p_55->z + p_56->z + p_65->z + p_66->z) / 4.0; + EXPECT_NEAR(p->z, avg_z, 1e-6); + const double avg_intensity = (p_55->intensity + p_56->intensity + p_65->intensity + p_66->intensity) / 4.0; + EXPECT_NEAR(p->intensity, avg_intensity, 1e-6); + } +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/localization_measurements/test/test_depth_image.test b/localization/localization_measurements/test/test_depth_image.test new file mode 100644 index 0000000000..8a92a45da5 --- /dev/null +++ b/localization/localization_measurements/test/test_depth_image.test @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/localization/localization_node/CMakeLists.txt b/localization/localization_node/CMakeLists.txt index d5f6478a2a..65ba82e505 100644 --- a/localization/localization_node/CMakeLists.txt +++ b/localization/localization_node/CMakeLists.txt @@ -56,6 +56,7 @@ catkin_package( tf2 sparse_mapping msg_conversions + ff_util ) diff --git a/localization/localization_node/package.xml b/localization/localization_node/package.xml index 8af7f9450e..8842c24650 100644 --- a/localization/localization_node/package.xml +++ b/localization/localization_node/package.xml @@ -26,6 +26,7 @@ tf2 sparse_mapping msg_conversions + ff_util roscpp message_runtime std_msgs @@ -38,6 +39,7 @@ tf2 sparse_mapping msg_conversions + ff_util diff --git a/localization/localization_node/tools/extract_image_bag.cc b/localization/localization_node/tools/extract_image_bag.cc index 78fcefb624..d780cdeb9c 100644 --- a/localization/localization_node/tools/extract_image_bag.cc +++ b/localization/localization_node/tools/extract_image_bag.cc @@ -122,9 +122,13 @@ int main(int argc, char ** argv) { // Convert to an opencv image cv::Mat image; try { + // Note that the assignment below is shallow, the image + // will be valid only for as long as image_msg is valid, + // which can result in issues in other situations. image = cv_bridge::toCvShare(image_msg, "bgr8")->image; } catch (cv_bridge::Exception const& e) { try { + // Note the same comment as earlier. image = cv_bridge::toCvShare(image_msg, "32FC1")->image; } catch (cv_bridge::Exception const& e) { LOG(ERROR) << "Unable to convert " << image_msg->encoding.c_str() diff --git a/localization/localization_node/tools/merge_bags.cc b/localization/localization_node/tools/merge_bags.cc index acd00d8456..420ff8a62d 100644 --- a/localization/localization_node/tools/merge_bags.cc +++ b/localization/localization_node/tools/merge_bags.cc @@ -82,14 +82,37 @@ int main(int argc, char ** argv) { } } + // First reorder the bags so that they are merged in the order of + // advanced time. + std::cout << "Determining the order of bags in time." << std::endl; + std::map time2bag; + for (int it = 1; it < argc; it++) { + std::string input_bag_file = argv[it]; + + // Get the topics from the input bag + std::vector topics; + readTopicsInBag(input_bag_file, topics); + + rosbag::Bag input_bag; + input_bag.open(input_bag_file, rosbag::bagmode::Read); + + rosbag::View view(input_bag, rosbag::TopicQuery(topics)); + for (rosbag::MessageInstance const m : view) { + time2bag[m.getTime().toSec()] = input_bag_file; + break; + } + + input_bag.close(); + } + // Open the output bag std::cout << "Opening the output bag file: " << FLAGS_output_bag << "\n"; rosbag::Bag output_bag; output_bag.open(FLAGS_output_bag, rosbag::bagmode::Write); - // Append the input bags - for (int it = 1; it < argc; it++) { - std::string input_bag_file = argv[it]; + // Append the input bags chronologically. + for (auto it = time2bag.begin(); it != time2bag.end(); it++) { + std::string input_bag_file = it->second; std::cout << "Opening the input bag file: " << input_bag_file << "\n"; diff --git a/localization/optimization_common/CMakeLists.txt b/localization/optimization_common/CMakeLists.txt index 2961c5543d..640081a4d6 100644 --- a/localization/optimization_common/CMakeLists.txt +++ b/localization/optimization_common/CMakeLists.txt @@ -26,18 +26,15 @@ find_package(catkin2 REQUIRED COMPONENTS localization_common ) -# Find OpenCV -SET(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/../../cmake") -find_package(OpenCV331 REQUIRED) - find_package(Eigen3 REQUIRED) # Non-linear optimizer +SET(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/../../cmake") find_package(Ceres REQUIRED) catkin_package( INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} ${CERES_LIBRARIES} ${OpenCV_LIBRARIES} + LIBRARIES ${PROJECT_NAME} ${CERES_LIBRARIES} CATKIN_DEPENDS localization_common ) @@ -51,7 +48,6 @@ include_directories( ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${CERES_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} ) # Declare C++ libraries @@ -59,7 +55,7 @@ add_library(${PROJECT_NAME} src/utilities.cc ) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${CERES_LIBRARIES} ${OpenCV_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${CERES_LIBRARIES}) ############# ## Install ## diff --git a/tools/calibration/include/calibration/optimization_params.h b/localization/optimization_common/include/optimization_common/optimization_params.h similarity index 79% rename from tools/calibration/include/calibration/optimization_params.h rename to localization/optimization_common/include/optimization_common/optimization_params.h index 4a9d5c5cd1..d44936b1bb 100644 --- a/tools/calibration/include/calibration/optimization_params.h +++ b/localization/optimization_common/include/optimization_common/optimization_params.h @@ -15,17 +15,17 @@ * License for the specific language governing permissions and limitations * under the License. */ -#ifndef CALIBRATION_OPTIMIZATION_PARAMS_H_ -#define CALIBRATION_OPTIMIZATION_PARAMS_H_ +#ifndef OPTIMIZATION_COMMON_OPTIMIZATION_PARAMS_H_ +#define OPTIMIZATION_COMMON_OPTIMIZATION_PARAMS_H_ #include -namespace calibration { +namespace optimization_common { struct OptimizationParams { ceres::Solver::Options solver_options; bool verbose; double huber_loss; }; -} // namespace calibration +} // namespace optimization_common -#endif // CALIBRATION_OPTIMIZATION_PARAMS_H_ +#endif // OPTIMIZATION_COMMON_OPTIMIZATION_PARAMS_H_ diff --git a/localization/optimization_common/include/optimization_common/residuals.h b/localization/optimization_common/include/optimization_common/residuals.h index 270f743930..02dd4ad5b8 100644 --- a/localization/optimization_common/include/optimization_common/residuals.h +++ b/localization/optimization_common/include/optimization_common/residuals.h @@ -33,99 +33,102 @@ namespace optimization_common { class PointToPointError { public: - PointToPointError(const Eigen::Vector3d& source_point, const Eigen::Vector3d& target_point) - : source_point_(source_point), target_point_(target_point) {} + PointToPointError(const Eigen::Vector3d& source_t_point, const Eigen::Vector3d& target_t_point) + : source_t_point_(source_t_point), target_t_point_(target_t_point) {} template - bool operator()(const T* relative_transform_data, T* point_to_point_error) const { - const auto relative_transform = Isometry3(relative_transform_data); + bool operator()(const T* target_T_source_data, T* point_to_point_error) const { + const auto target_T_source = Isometry3(target_T_source_data); // Compute error - const Eigen::Matrix transformed_source_point = relative_transform * source_point_.cast(); - point_to_point_error[0] = transformed_source_point[0] - target_point_[0]; - point_to_point_error[1] = transformed_source_point[1] - target_point_[1]; - point_to_point_error[2] = transformed_source_point[2] - target_point_[2]; + const Eigen::Matrix estimated_target_t_point = target_T_source * source_t_point_.cast(); + point_to_point_error[0] = estimated_target_t_point[0] - target_t_point_[0]; + point_to_point_error[1] = estimated_target_t_point[1] - target_t_point_[1]; + point_to_point_error[2] = estimated_target_t_point[2] - target_t_point_[2]; return true; } - static void AddCostFunction(const Eigen::Vector3d& source_point, const Eigen::Vector3d& target_point, - Eigen::Matrix& relative_transform, ceres::Problem& problem) { + static void AddCostFunction(const Eigen::Vector3d& source_t_point, const Eigen::Vector3d& target_t_point, + Eigen::Matrix& target_T_source, ceres::Problem& problem) { ceres::LossFunction* huber_loss = new ceres::HuberLoss(1.345); ceres::CostFunction* point_to_point_cost_function = - new ceres::AutoDiffCostFunction(new PointToPointError(source_point, target_point)); - problem.AddResidualBlock(point_to_point_cost_function, huber_loss, relative_transform.data()); + new ceres::AutoDiffCostFunction(new PointToPointError(source_t_point, target_t_point)); + problem.AddResidualBlock(point_to_point_cost_function, huber_loss, target_T_source.data()); } private: - Eigen::Vector3d source_point_; - Eigen::Vector3d target_point_; + Eigen::Vector3d source_t_point_; + Eigen::Vector3d target_t_point_; }; class PointToPlaneError { public: - PointToPlaneError(const Eigen::Vector3d& source_point, const Eigen::Vector3d& target_point, + PointToPlaneError(const Eigen::Vector3d& source_t_point, const Eigen::Vector3d& target_t_point, const Eigen::Vector3d& target_normal) - : source_point_(source_point), target_point_(target_point), target_normal_(target_normal) {} + : source_t_point_(source_t_point), target_t_point_(target_t_point), target_normal_(target_normal) {} template - bool operator()(const T* relative_transform_data, T* point_to_plane_error) const { - const auto relative_transform = Isometry3(relative_transform_data); + bool operator()(const T* target_T_source_data, T* point_to_plane_error) const { + const auto target_T_source = Isometry3(target_T_source_data); // Compute error - const Eigen::Matrix transformed_source_point = relative_transform * source_point_.cast(); - const Eigen::Matrix transformed_source_point_to_target_point = - transformed_source_point - target_point_.cast(); - point_to_plane_error[0] = transformed_source_point_to_target_point.dot(target_normal_.cast()); + const Eigen::Matrix estimated_target_t_point = target_T_source * source_t_point_.cast(); + const Eigen::Matrix target_F_point_t_estimated_point = + estimated_target_t_point - target_t_point_.cast(); + point_to_plane_error[0] = target_F_point_t_estimated_point.dot(target_normal_.cast()); return true; } - static void AddCostFunction(const Eigen::Vector3d& source_point, const Eigen::Vector3d& target_point, - const Eigen::Vector3d& target_normal, Eigen::Matrix& relative_transform, + static void AddCostFunction(const Eigen::Vector3d& source_t_point, const Eigen::Vector3d& target_t_point, + const Eigen::Vector3d& target_normal, Eigen::Matrix& target_T_source, ceres::Problem& problem) { ceres::LossFunction* huber_loss = new ceres::HuberLoss(1.345); ceres::CostFunction* point_to_plane_cost_function = new ceres::AutoDiffCostFunction( - new PointToPlaneError(source_point, target_point, target_normal)); - problem.AddResidualBlock(point_to_plane_cost_function, huber_loss, relative_transform.data()); + new PointToPlaneError(source_t_point, target_t_point, target_normal)); + problem.AddResidualBlock(point_to_plane_cost_function, huber_loss, target_T_source.data()); } private: - Eigen::Vector3d source_point_; - Eigen::Vector3d target_point_; + Eigen::Vector3d source_t_point_; + Eigen::Vector3d target_t_point_; Eigen::Vector3d target_normal_; }; class SymmetricPointToPlaneError { public: - SymmetricPointToPlaneError(const Eigen::Vector3d& source_point, const Eigen::Vector3d& target_point, + SymmetricPointToPlaneError(const Eigen::Vector3d& source_t_point, const Eigen::Vector3d& target_t_point, const Eigen::Vector3d& source_normal, const Eigen::Vector3d& target_normal) - : source_point_(source_point), - target_point_(target_point), + : source_t_point_(source_t_point), + target_t_point_(target_t_point), source_normal_(source_normal), target_normal_(target_normal) {} template - bool operator()(const T* relative_transform_data, T* symmetric_point_to_plane_error) const { - const auto relative_transform = Isometry3(relative_transform_data); + bool operator()(const T* target_T_source_data, T* symmetric_point_to_plane_error) const { + const auto target_T_source = Isometry3(target_T_source_data); // Compute error - const Eigen::Matrix transformed_source_point = relative_transform * source_point_.cast(); - const Eigen::Matrix transformed_source_point_to_target_point = - transformed_source_point - target_point_.cast(); - symmetric_point_to_plane_error[0] = transformed_source_point_to_target_point.dot(source_normal_.cast()); - symmetric_point_to_plane_error[1] = transformed_source_point_to_target_point.dot(target_normal_.cast()); + const Eigen::Matrix estimated_target_t_point = target_T_source * source_t_point_.cast(); + const Eigen::Matrix target_F_point_t_estimated_point = + estimated_target_t_point - target_t_point_.cast(); + const Eigen::Matrix estimated_source_t_point = target_T_source.inverse() * target_t_point_.cast(); + const Eigen::Matrix source_F_point_t_estimated_point = + estimated_source_t_point - source_t_point_.cast(); + symmetric_point_to_plane_error[0] = source_F_point_t_estimated_point.dot(source_normal_.cast()); + symmetric_point_to_plane_error[1] = target_F_point_t_estimated_point.dot(target_normal_.cast()); return true; } - static void AddCostFunction(const Eigen::Vector3d& source_point, const Eigen::Vector3d& target_point, + static void AddCostFunction(const Eigen::Vector3d& source_t_point, const Eigen::Vector3d& target_t_point, const Eigen::Vector3d& source_normal, const Eigen::Vector3d& target_normal, - Eigen::Matrix& relative_transform, ceres::Problem& problem) { + Eigen::Matrix& target_T_source, ceres::Problem& problem) { ceres::LossFunction* huber_loss = new ceres::HuberLoss(1.345); ceres::CostFunction* symmetric_point_to_plane_cost_function = new ceres::AutoDiffCostFunction( - new SymmetricPointToPlaneError(source_point, target_point, source_normal, target_normal)); - problem.AddResidualBlock(symmetric_point_to_plane_cost_function, huber_loss, relative_transform.data()); + new SymmetricPointToPlaneError(source_t_point, target_t_point, source_normal, target_normal)); + problem.AddResidualBlock(symmetric_point_to_plane_cost_function, huber_loss, target_T_source.data()); } private: - Eigen::Vector3d source_point_; - Eigen::Vector3d target_point_; + Eigen::Vector3d source_t_point_; + Eigen::Vector3d target_t_point_; Eigen::Vector3d source_normal_; Eigen::Vector3d target_normal_; }; diff --git a/localization/optimization_common/include/optimization_common/utilities.h b/localization/optimization_common/include/optimization_common/utilities.h index fb95552d7e..e10f0665a7 100644 --- a/localization/optimization_common/include/optimization_common/utilities.h +++ b/localization/optimization_common/include/optimization_common/utilities.h @@ -56,13 +56,6 @@ Eigen::Matrix Intrinsics(const T* intrinsics_data); template Eigen::Matrix Intrinsics(const T* focal_lengths, const T* principal_points); -template -Eigen::Matrix RelativeCoordinates(const Eigen::Matrix& absolute_point, - const Eigen::Matrix& intrinsics); -template -Eigen::Matrix AbsoluteCoordinates(const Eigen::Matrix& relative_point, - const Eigen::Matrix& intrinsics); - void AddParameterBlock(const int num_parameters, double* const parameters, ceres::Problem& problem, const bool set_constant = false); @@ -127,31 +120,6 @@ Eigen::Matrix Intrinsics(const T* focal_lengths, const T* principal_poi intrinsics(1, 2) = principal_points[1]; return intrinsics; } - -template -Eigen::Matrix RelativeCoordinates(const Eigen::Matrix& absolute_point, - const Eigen::Matrix& intrinsics) { - const T& f_x = intrinsics(0, 0); - const T& f_y = intrinsics(1, 1); - const T& p_x = intrinsics(0, 2); - const T& p_y = intrinsics(1, 2); - - const T& x = absolute_point[0]; - const T& y = absolute_point[1]; - const T relative_x = (x - p_x) / f_x; - const T relative_y = (y - p_y) / f_y; - return Eigen::Matrix(relative_x, relative_y); -} - -template -Eigen::Matrix AbsoluteCoordinates(const Eigen::Matrix& relative_point, - const Eigen::Matrix& intrinsics) { - const T& f_x = intrinsics(0, 0); - const T& f_y = intrinsics(1, 1); - const T& p_x = intrinsics(0, 2); - const T& p_y = intrinsics(1, 2); - return Eigen::Matrix(relative_point[0] * f_x + p_x, relative_point[1] * f_y + p_y); -} } // namespace optimization_common #endif // OPTIMIZATION_COMMON_UTILITIES_H_ diff --git a/localization/optimization_common/readme.md b/localization/optimization_common/readme.md new file mode 100644 index 0000000000..b7eb08bb7d --- /dev/null +++ b/localization/optimization_common/readme.md @@ -0,0 +1,12 @@ +\page optimizationcommon Optimization Common + +# Package Overview + +### Residuals +Provides various error functions (point to point, point to plane, reprojection, etc.) for usage in Ceres-based nonlinear optimization problems. + +### SE3 Local Parameterization +Provides a local parameterization for the SE3 Lie Group that performs on-manifold updates using the axis-angle Lie Algebra. Axis-angle updates are provided with an array in compact axis-angle format (where the norm of the 3d array is the angle and the normalized 3d array is the axis) and applied using right multiplication on the given SE3 element, mimicking the right multiplicative updates used by the GTSAM library. + +### Utilities +Provides helper functions to convert between minimal and non-minimal representations for optimization variables along with other optimization helper functions. diff --git a/localization/point_cloud_common/CMakeLists.txt b/localization/point_cloud_common/CMakeLists.txt new file mode 100644 index 0000000000..d365d7f289 --- /dev/null +++ b/localization/point_cloud_common/CMakeLists.txt @@ -0,0 +1,112 @@ +#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. + +cmake_minimum_required(VERSION 3.0) +project(point_cloud_common) + +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + ff_util + localization_common + msg_conversions + optimization_common +) + +find_package(Eigen3 REQUIRED) + +find_package(Ceres REQUIRED) + +find_package(PCL REQUIRED COMPONENTS common features filters octree registration search) + +find_package(pcl_conversions REQUIRED) + +catkin_package( + INCLUDE_DIRS include ${CERES_INCLUDE_DIRS} ${GTSAM_INCLUDE_DIR} ${PCL_INCLUDE_DIRS} ${pcl_conversions_INCLUDE_DIRS} + LIBRARIES ${PROJECT_NAME} ${CERES_LIBRARIES} ${PCL_LIBRARIES} ${GTSAM_LIBRARIES} + CATKIN_DEPENDS roscpp ff_msgs ff_util localization_common msg_conversions optimization_common + DEPENDS gtsam pcl +) + +########### +## Build ## +########### +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${CERES_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} + ${pcl_conversions_INCLUDE_DIRS} +) + +# Declare C++ libraries +add_library(${PROJECT_NAME} + src/parameter_reader.cc + src/point_cloud_with_known_correspondences_aligner.cc + src/test_utilities.cc + src/utilities.cc +) +add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${PCL_LIBRARIES} gtsam) + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest_gtest(test_point_cloud_common_utilities + test/test_point_cloud_common_utilities.test + test/test_point_cloud_common_utilities.cc + ) + target_link_libraries(test_point_cloud_common_utilities + ${PROJECT_NAME} ${catkin_LIBRARIES} gtsam + ) + add_rostest_gtest(test_point_to_plane_icp + test/test_point_to_plane_icp.test + test/test_point_to_plane_icp.cc + ) + target_link_libraries(test_point_to_plane_icp + ${PROJECT_NAME} ${catkin_LIBRARIES} + ) + add_rostest_gtest(test_point_cloud_with_known_correspondences_aligner + test/test_point_cloud_with_known_correspondences_aligner.test + test/test_point_cloud_with_known_correspondences_aligner.cc + ) + target_link_libraries(test_point_cloud_with_known_correspondences_aligner + ${PROJECT_NAME} ${catkin_LIBRARIES} + ) + +endif() + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) diff --git a/localization/point_cloud_common/include/point_cloud_common/correspondence_rejection_surface_normal2.h b/localization/point_cloud_common/include/point_cloud_common/correspondence_rejection_surface_normal2.h new file mode 100644 index 0000000000..7688e4199b --- /dev/null +++ b/localization/point_cloud_common/include/point_cloud_common/correspondence_rejection_surface_normal2.h @@ -0,0 +1,319 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ +#ifndef PCL_REGISTRATION_CORRESPONDENCE_REJECTION_SURFACE_NORMAL2_H_ // NOLINT +#define PCL_REGISTRATION_CORRESPONDENCE_REJECTION_SURFACE_NORMAL2_H_ // NOLINT + +#include +#include + +namespace pcl { +namespace registration { +// TODO(rsoussan): This fixes a bug in PCL for setSourceNormals, where PCL hardcodes the normal type to pcl::Normal +// when it should use NormalT. Switch back to PCL implementation when this is fixed. Additionally this should be +// either fixed in setTargetNormals or requiresTargetNormals should return false (ideally the second, and +// requires*Points should also be false). +/** + * @b CorrespondenceRejectorSurfaceNormal2 implements a simple correspondence + * rejection method based on the angle between the normals at correspondent points. + * + * \note If \ref setInputCloud and \ref setInputTarget are given, then the + * distances between correspondences will be estimated using the given XYZ + * data, and not read from the set of input correspondences. + * + * \author Aravindhan K Krishnan (original code from libpointmatcher: https://github.com/ethz-asl/libpointmatcher) + * \ingroup registration + */ +template +class PCL_EXPORTS CorrespondenceRejectorSurfaceNormal2 : public CorrespondenceRejector { + using CorrespondenceRejector::getClassName; + using CorrespondenceRejector::input_correspondences_; + using CorrespondenceRejector::rejection_name_; + + public: + typedef boost::shared_ptr Ptr; + typedef boost::shared_ptr ConstPtr; + + /** \brief Empty constructor. Sets the threshold to 1.0. */ + CorrespondenceRejectorSurfaceNormal2() : threshold_(1.0), data_container_() { + rejection_name_ = "CorrespondenceRejectorSurfaceNormal2"; + } + + /** \brief Get a list of valid correspondences after rejection from the original set of correspondences. + * \param[in] original_correspondences the set of initial correspondences given + * \param[out] remaining_correspondences the resultant filtered set of remaining correspondences + */ + // void + // getRemainingCorrespondences (const pcl::Correspondences& original_correspondences, + // pcl::Correspondences& remaining_correspondences); + + /** \brief Set the thresholding angle between the normals for correspondence rejection. + * \param[in] threshold cosine of the thresholding angle between the normals for rejection + */ + inline void setThreshold(double threshold) { threshold_ = threshold; } + + /** \brief Get the thresholding angle between the normals for correspondence rejection. */ + inline double getThreshold() const { return threshold_; } + + void getRemainingCorrespondences(const pcl::Correspondences& original_correspondences, + pcl::Correspondences& remaining_correspondences) { + if (!data_container_) { + PCL_ERROR("[pcl::registratin::%s::getRemainingCorrespondences] DataContainer object is not initialized!\n", + getClassName().c_str()); + return; + } + + unsigned int number_valid_correspondences = 0; + remaining_correspondences.resize(original_correspondences.size()); + + // Test each correspondence + for (size_t i = 0; i < original_correspondences.size(); ++i) { + // TODO(rsoussan): Abs val? + if (std::abs(boost::static_pointer_cast >(data_container_) + ->getCorrespondenceScoreFromNormals(original_correspondences[i])) > threshold_) { + remaining_correspondences[number_valid_correspondences++] = original_correspondences[i]; + } + } + remaining_correspondences.resize(number_valid_correspondences); + } + + /** \brief Initialize the data container object for the point type and the normal type. */ + inline void initializeDataContainer() { + data_container_.reset(new DataContainer); + } + + /** \brief Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence distance. + * \param[in] input a cloud containing XYZ data + */ + inline void setInputCloud(const typename pcl::PointCloud::ConstPtr& input) { + PCL_WARN("[pcl::registration::%s::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.\n", + getClassName().c_str()); + if (!data_container_) { + PCL_ERROR( + "[pcl::registration::%s::setInputCloud] Initialize the data container object by calling intializeDataContainer " + "() before using this function.\n", + getClassName().c_str()); + return; + } + boost::static_pointer_cast >(data_container_)->setInputSource(input); + } + + /** \brief Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence distance. + * \param[in] input a cloud containing XYZ data + */ + inline void setInputSource(const typename pcl::PointCloud::ConstPtr& input) { + if (!data_container_) { + PCL_ERROR( + "[pcl::registration::%s::setInputCloud] Initialize the data container object by calling intializeDataContainer " + "() before using this function.\n", + getClassName().c_str()); + return; + } + boost::static_pointer_cast >(data_container_)->setInputSource(input); + } + + /** \brief Get the target input point cloud */ + inline typename pcl::PointCloud::ConstPtr getInputSource() const { + if (!data_container_) { + PCL_ERROR( + "[pcl::registration::%s::getInputSource] Initialize the data container object by calling " + "intializeDataContainer () before using this function.\n", + getClassName().c_str()); + return; + } + return (boost::static_pointer_cast >(data_container_)->getInputSource()); + } + + /** \brief Provide a target point cloud dataset (must contain XYZ data!), used to compute the correspondence distance. + * \param[in] target a cloud containing XYZ data + */ + inline void setInputTarget(const typename pcl::PointCloud::ConstPtr& target) { + if (!data_container_) { + PCL_ERROR( + "[pcl::registration::%s::setInputTarget] Initialize the data container object by calling " + "intializeDataContainer () before using this function.\n", + getClassName().c_str()); + return; + } + boost::static_pointer_cast >(data_container_)->setInputTarget(target); + } + + /** \brief Provide a pointer to the search object used to find correspondences in + * the target cloud. + * \param[in] tree a pointer to the spatial search object. + * \param[in] force_no_recompute If set to true, this tree will NEVER be + * recomputed, regardless of calls to setInputTarget. Only use if you are + * confident that the tree will be set correctly. + */ + inline void setSearchMethodTarget(const boost::shared_ptr >& tree, + bool force_no_recompute = false) { + boost::static_pointer_cast >(data_container_) + ->setSearchMethodTarget(tree, force_no_recompute); + } + + /** \brief Get the target input point cloud */ + inline typename pcl::PointCloud::ConstPtr getInputTarget() const { + if (!data_container_) { + PCL_ERROR( + "[pcl::registration::%s::getInputTarget] Initialize the data container object by calling " + "intializeDataContainer () before using this function.\n", + getClassName().c_str()); + return; + } + return (boost::static_pointer_cast >(data_container_)->getInputTarget()); + } + + /** \brief Set the normals computed on the input point cloud + * \param[in] normals the normals computed for the input cloud + */ + inline void setInputNormals(const typename pcl::PointCloud::ConstPtr& normals) { + if (!data_container_) { + PCL_ERROR( + "[pcl::registration::%s::setInputNormals] Initialize the data container object by calling " + "intializeDataContainer () before using this function.\n", + getClassName().c_str()); + return; + } + boost::static_pointer_cast >(data_container_) + ->setInputNormals(normals); + } + + /** \brief Get the normals computed on the input point cloud */ + inline typename pcl::PointCloud::Ptr getInputNormals() const { + if (!data_container_) { + PCL_ERROR( + "[pcl::registration::%s::getInputNormals] Initialize the data container object by calling " + "intializeDataContainer () before using this function.\n", + getClassName().c_str()); + return; + } + return (boost::static_pointer_cast >(data_container_) + ->getInputNormals()); + } + + /** \brief Set the normals computed on the target point cloud + * \param[in] normals the normals computed for the input cloud + */ + inline void setTargetNormals(const typename pcl::PointCloud::ConstPtr& normals) { + if (!data_container_) { + PCL_ERROR( + "[pcl::registration::%s::setTargetNormals] Initialize the data container object by calling " + "intializeDataContainer () before using this function.\n", + getClassName().c_str()); + return; + } + boost::static_pointer_cast >(data_container_) + ->setTargetNormals(normals); + } + + /** \brief Get the normals computed on the target point cloud */ + inline typename pcl::PointCloud::Ptr getTargetNormals() const { + if (!data_container_) { + PCL_ERROR( + "[pcl::registration::%s::getTargetNormals] Initialize the data container object by calling " + "intializeDataContainer () before using this function.\n", + getClassName().c_str()); + return; + } + return (boost::static_pointer_cast >(data_container_) + ->getTargetNormals()); + } + + /** \brief See if this rejector requires source points */ + bool requiresSourcePoints() const { return (false); } + + /** \brief Blob method for setting the source cloud */ + void setSourcePoints(pcl::PCLPointCloud2::ConstPtr cloud2) { + if (!data_container_) initializeDataContainer(); + typename PointCloud::Ptr cloud(new PointCloud); + fromPCLPointCloud2(*cloud2, *cloud); + setInputSource(cloud); + } + + /** \brief See if this rejector requires a target cloud */ + bool requiresTargetPoints() const { return (false); } + + /** \brief Method for setting the target cloud */ + void setTargetPoints(pcl::PCLPointCloud2::ConstPtr cloud2) { + if (!data_container_) initializeDataContainer(); + typename PointCloud::Ptr cloud(new PointCloud); + fromPCLPointCloud2(*cloud2, *cloud); + setInputTarget(cloud); + } + + /** \brief See if this rejector requires source normals */ + bool requiresSourceNormals() const { return (true); } + + /** \brief Blob method for setting the source normals */ + void setSourceNormals(pcl::PCLPointCloud2::ConstPtr cloud2) { + if (!data_container_) initializeDataContainer(); + typename PointCloud::Ptr cloud(new PointCloud); + fromPCLPointCloud2(*cloud2, *cloud); + setInputNormals(cloud); + } + + /** \brief See if this rejector requires target normals*/ + bool requiresTargetNormals() const { return (false); } + + /** \brief Method for setting the target normals */ + void setTargetNormals(pcl::PCLPointCloud2::ConstPtr cloud2) { + if (!data_container_) initializeDataContainer(); + typename PointCloud::Ptr cloud(new PointCloud); + fromPCLPointCloud2(*cloud2, *cloud); + setTargetNormals(cloud); + } + + protected: + /** \brief Apply the rejection algorithm. + * \param[out] correspondences the set of resultant correspondences. + */ + inline void applyRejection(pcl::Correspondences& correspondences) { + getRemainingCorrespondences(*input_correspondences_, correspondences); + } + + /** \brief The median distance threshold between two correspondent points in source <-> target. */ + double threshold_; + + typedef boost::shared_ptr DataContainerPtr; + /** \brief A pointer to the DataContainer object containing the input and target point clouds */ + DataContainerPtr data_container_; +}; +} // namespace registration +} // namespace pcl + +#endif // NOLINT diff --git a/localization/point_cloud_common/include/point_cloud_common/icp_correspondences.h b/localization/point_cloud_common/include/point_cloud_common/icp_correspondences.h new file mode 100644 index 0000000000..0ca228400f --- /dev/null +++ b/localization/point_cloud_common/include/point_cloud_common/icp_correspondences.h @@ -0,0 +1,39 @@ +/* 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. + */ +#ifndef POINT_CLOUD_COMMON_ICP_CORRESPONDENCES_H_ +#define POINT_CLOUD_COMMON_ICP_CORRESPONDENCES_H_ + +#include + +#include + +namespace point_cloud_common { +struct ICPCorrespondences { + ICPCorrespondences(const std::vector& source_points, + const std::vector& target_points, + const std::vector& target_normals) + : source_points(source_points), target_points(target_points), target_normals(target_normals) {} + size_t size() const { return source_points.size(); } + + std::vector source_points; + std::vector target_points; + std::vector target_normals; +}; +} // namespace point_cloud_common + +#endif // POINT_CLOUD_COMMON_ICP_CORRESPONDENCES_H_ diff --git a/localization/point_cloud_common/include/point_cloud_common/organized_neighbor2.h b/localization/point_cloud_common/include/point_cloud_common/organized_neighbor2.h new file mode 100644 index 0000000000..2a4feba2dc --- /dev/null +++ b/localization/point_cloud_common/include/point_cloud_common/organized_neighbor2.h @@ -0,0 +1,209 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef POINT_CLOUD_COMMON_ORGANIZED_NEIGHBOR2_H_ +#define POINT_CLOUD_COMMON_ORGANIZED_NEIGHBOR2_H_ + +#include +#include +#include +#include + +#include +#include +#include + +// Modified version of pcl organized neighbor to enable preseting the camera intrinsics matrix. +namespace pcl { +namespace search { +/** \brief OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds. + * \author Radu B. Rusu, Julius Kammerl, Suat Gedikli, Koen Buys + * \ingroup search + */ +template +class OrganizedNeighbor2 : public pcl::search::Search { + public: + // public typedefs + typedef pcl::PointCloud PointCloud; + typedef boost::shared_ptr PointCloudPtr; + + typedef boost::shared_ptr PointCloudConstPtr; + typedef boost::shared_ptr > IndicesConstPtr; + + typedef boost::shared_ptr > Ptr; + typedef boost::shared_ptr > ConstPtr; + + using pcl::search::Search::indices_; + using pcl::search::Search::sorted_results_; + using pcl::search::Search::input_; + + /** \brief Constructor + * \param[in] sorted_results whether the results should be return sorted in ascending order on the distances or not. + * This applies only for radius search, since knn always returns sorted resutls + */ + explicit OrganizedNeighbor2(bool sorted_results = false) + : Search("OrganizedNeighbor2", sorted_results), mask_() {} + + /** \brief Empty deconstructor. */ + virtual ~OrganizedNeighbor2() {} + + /** \brief Provide a pointer to the input data set, if user has focal length he must set it before calling this + * \param[in] cloud the const boost shared pointer to a PointCloud message + * \param[in] indices the const boost shared pointer to PointIndices + */ + virtual void setInputCloud(const PointCloudConstPtr& cloud, const IndicesConstPtr& indices = IndicesConstPtr()) { + input_ = cloud; + + mask_.resize(input_->size()); + input_ = cloud; + indices_ = indices; + + if (indices_.get() != NULL && indices_->size() != 0) { + mask_.assign(input_->size(), 0); + for (std::vector::const_iterator iIt = indices_->begin(); iIt != indices_->end(); ++iIt) mask_[*iIt] = 1; + } else { + mask_.assign(input_->size(), 1); + } + } + + /** \brief Search for all neighbors of query point that are within a given radius. + * \param[in] p_q the given query point + * \param[in] radius the radius of the sphere bounding all of p_q's neighbors + * \param[out] k_indices the resultant indices of the neighboring points + * \param[out] k_sqr_distances the resultant squared distances to the neighboring points + * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to + * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be + * returned. + * \return number of neighbors found in radius + */ + int radiusSearch(const PointT& p_q, double radius, std::vector& k_indices, std::vector& k_sqr_distances, + unsigned int max_nn = 0) const; + + /** \brief Search for the k-nearest neighbors for a given query point. + * \note limiting the maximum search radius (with setMaxDistance) can lead to a significant improvement in search + * speed \param[in] p_q the given query point (\ref setInputCloud must be given a-priori!) \param[in] k the number of + * neighbors to search for (used only if horizontal and vertical window not given already!) \param[out] k_indices the + * resultant point indices (must be resized to \a k beforehand!) \param[out] k_sqr_distances \note this function does + * not return distances \return number of neighbors found + * @todo still need to implements this functionality + */ + int nearestKSearch(const PointT& p_q, int k, std::vector& k_indices, std::vector& k_sqr_distances) const; + + /** \brief projects a point into the image + * \param[in] p point in 3D World Coordinate Frame to be projected onto the image plane + * @return The 2D projected point in pixel coordinates (u,v) + */ + Eigen::Vector2f projectPoint(const Eigen::Vector3f& p) const; + + void setIntrinsicsMatrix(const Eigen::Matrix3d& intrinsics_matrix) { + intrinsics_matrix_ = intrinsics_matrix.cast(); + KR_KRT_ = intrinsics_matrix_ * intrinsics_matrix_.transpose(); + } + + protected: + struct Entry { + Entry(int idx, float dist) : index(idx), distance(dist) {} + Entry() : index(0), distance(0) {} + unsigned index; + float distance; + + inline bool operator<(const Entry& other) const { return (distance < other.distance); } + }; + + /** \brief test if point given by index is among the k NN in results to the query point. + * \param[in] query query point + * \param[in] k number of maximum nn interested in + * \param[in] queue priority queue with k NN + * \param[in] index index on point to be tested + * \return wheter the top element changed or not. + */ + inline bool testPoint(const PointT& query, unsigned k, std::priority_queue& queue, unsigned index) const { + const PointT& point = input_->points[index]; + if (mask_[index] && pcl_isfinite(point.x)) { + // float squared_distance = (point.getVector3fMap () - query.getVector3fMap ()).squaredNorm (); + float dist_x = point.x - query.x; + float dist_y = point.y - query.y; + float dist_z = point.z - query.z; + float squared_distance = dist_x * dist_x + dist_y * dist_y + dist_z * dist_z; + if (queue.size() < k) { + queue.push(Entry(index, squared_distance)); + } else if (queue.top().distance > squared_distance) { + queue.pop(); + queue.push(Entry(index, squared_distance)); + return true; // top element has changed! + } + } + return false; + } + + inline void clipRange(int& begin, int& end, int min, int max) const { + begin = std::max(std::min(begin, max), min); + end = std::min(std::max(end, min), max); + } + + /** \brief Obtain a search box in 2D from a sphere with a radius in 3D + * \param[in] point the query point (sphere center) + * \param[in] squared_radius the squared sphere radius + * \param[out] minX the min X box coordinate + * \param[out] minY the min Y box coordinate + * \param[out] maxX the max X box coordinate + * \param[out] maxY the max Y box coordinate + */ + void getProjectedRadiusSearchBox(const PointT& point, float squared_radius, unsigned& minX, unsigned& minY, + unsigned& maxX, unsigned& maxY) const; + + /** \brief the intrinsics matrix. */ + Eigen::Matrix3f intrinsics_matrix_; + + Eigen::Matrix3f KR_KRT_; + + /** \brief mask, indicating whether the point was in the indices list or not.*/ + std::vector mask_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; +} // namespace search +} // namespace pcl + +#ifdef PCL_NO_PRECOMPILE +#include +#endif + +#endif // POINT_CLOUD_COMMON_ORGANIZED_NEIGHBOR2_H_ diff --git a/localization/point_cloud_common/include/point_cloud_common/organized_neighbor2_impl.h b/localization/point_cloud_common/include/point_cloud_common/organized_neighbor2_impl.h new file mode 100644 index 0000000000..a38a233da9 --- /dev/null +++ b/localization/point_cloud_common/include/point_cloud_common/organized_neighbor2_impl.h @@ -0,0 +1,296 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +#ifndef POINT_CLOUD_COMMON_ORGANIZED_NEIGHBOR2_IMPL_H_ +#define POINT_CLOUD_COMMON_ORGANIZED_NEIGHBOR2_IMPL_H_ + +#include +#include +#include +#include + +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template +int pcl::search::OrganizedNeighbor2::radiusSearch(const PointT& query, const double radius, + std::vector& k_indices, + std::vector& k_sqr_distances, + unsigned int max_nn) const { + // NAN test + assert(isFinite(query) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); + + // search window + unsigned left, right, top, bottom; + // unsigned x, y, idx; + float squared_distance; + double squared_radius; + + k_indices.clear(); + k_sqr_distances.clear(); + + squared_radius = radius * radius; + + this->getProjectedRadiusSearchBox(query, static_cast(squared_radius), left, right, top, bottom); + + // iterate over search box + if (max_nn == 0 || max_nn >= static_cast(input_->points.size())) + max_nn = static_cast(input_->points.size()); + + k_indices.reserve(max_nn); + k_sqr_distances.reserve(max_nn); + + unsigned yEnd = (bottom + 1) * input_->width + right + 1; + register unsigned idx = top * input_->width + left; + unsigned skip = input_->width - right + left - 1; + unsigned xEnd = idx - left + right + 1; + + for (; xEnd != yEnd; idx += skip, xEnd += input_->width) { + for (; idx < xEnd; ++idx) { + if (!mask_[idx] || !isFinite(input_->points[idx])) continue; + + float dist_x = input_->points[idx].x - query.x; + float dist_y = input_->points[idx].y - query.y; + float dist_z = input_->points[idx].z - query.z; + squared_distance = dist_x * dist_x + dist_y * dist_y + dist_z * dist_z; + // squared_distance = (input_->points[idx].getVector3fMap () - query.getVector3fMap ()).squaredNorm (); + if (squared_distance <= squared_radius) { + k_indices.push_back(idx); + k_sqr_distances.push_back(squared_distance); + // already done ? + if (k_indices.size() == max_nn) { + if (sorted_results_) this->sortResults(k_indices, k_sqr_distances); + return (max_nn); + } + } + } + } + if (sorted_results_) this->sortResults(k_indices, k_sqr_distances); + return (static_cast(k_indices.size())); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +int pcl::search::OrganizedNeighbor2::nearestKSearch(const PointT& query, int k, std::vector& k_indices, + std::vector& k_sqr_distances) const { + assert(isFinite(query) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); + if (k < 1) { + k_indices.clear(); + k_sqr_distances.clear(); + return (0); + } + + const Eigen::Vector3f queryvec(query.x, query.y, query.z); + // project query point on the image plane + const Eigen::Vector2f q = projectPoint(queryvec); + int xBegin = static_cast(q[0] + 0.5f); + int yBegin = static_cast(q[1] + 0.5f); + int xEnd = xBegin + 1; // end is the pixel that is not used anymore, like in iterators + int yEnd = yBegin + 1; + + // the search window. This is supposed to shrink within the iterations + unsigned left = 0; + unsigned right = input_->width - 1; + unsigned top = 0; + unsigned bottom = input_->height - 1; + + std::priority_queue results; + // std::vector k_results; + // k_results.reserve (k); + // add point laying on the projection of the query point. + if (xBegin >= 0 && xBegin < static_cast(input_->width) && yBegin >= 0 && + yBegin < static_cast(input_->height)) { + testPoint(query, k, results, yBegin * input_->width + xBegin); + } else { // point lys + // find the box that touches the image border -> dont waste time evaluating boxes that are completely outside the + // image! + int dist = std::numeric_limits::max(); + + if (xBegin < 0) + dist = -xBegin; + else if (xBegin >= static_cast(input_->width)) + dist = xBegin - static_cast(input_->width); + + if (yBegin < 0) + dist = std::min(dist, -yBegin); + else if (yBegin >= static_cast(input_->height)) + dist = std::min(dist, yBegin - static_cast(input_->height)); + + xBegin -= dist; + xEnd += dist; + + yBegin -= dist; + yEnd += dist; + } + + // stop used as isChanged as well as stop. + bool stop = false; + do { + // increment box size + --xBegin; + ++xEnd; + --yBegin; + ++yEnd; + + // the range in x-direction which intersects with the image width + int xFrom = xBegin; + int xTo = xEnd; + clipRange(xFrom, xTo, 0, input_->width); + + // if x-extend is not 0 + if (xTo > xFrom) { + // if upper line of the rectangle is visible and x-extend is not 0 + if (yBegin >= 0 && yBegin < static_cast(input_->height)) { + int idx = yBegin * input_->width + xFrom; + int idxTo = idx + xTo - xFrom; + for (; idx < idxTo; ++idx) stop = testPoint(query, k, results, idx) || stop; + } + + // the row yEnd does NOT belong to the box -> last row = yEnd - 1 + // if lower line of the rectangle is visible + if (yEnd > 0 && yEnd <= static_cast(input_->height)) { + int idx = (yEnd - 1) * input_->width + xFrom; + int idxTo = idx + xTo - xFrom; + + for (; idx < idxTo; ++idx) stop = testPoint(query, k, results, idx) || stop; + } + + // skip first row and last row (already handled above) + int yFrom = yBegin + 1; + int yTo = yEnd - 1; + clipRange(yFrom, yTo, 0, input_->height); + + // if we have lines in between that are also visible + if (yFrom < yTo) { + if (xBegin >= 0 && xBegin < static_cast(input_->width)) { + int idx = yFrom * input_->width + xBegin; + int idxTo = yTo * input_->width + xBegin; + + for (; idx < idxTo; idx += input_->width) stop = testPoint(query, k, results, idx) || stop; + } + + if (xEnd > 0 && xEnd <= static_cast(input_->width)) { + int idx = yFrom * input_->width + xEnd - 1; + int idxTo = yTo * input_->width + xEnd - 1; + + for (; idx < idxTo; idx += input_->width) stop = testPoint(query, k, results, idx) || stop; + } + } + // stop here means that the k-nearest neighbor changed -> recalculate bounding box of ellipse. + if (stop) getProjectedRadiusSearchBox(query, results.top().distance, left, right, top, bottom); + } + // now we use it as stop flag -> if bounding box is completely within the already examined search box were done! + stop = (static_cast(left) >= xBegin && static_cast(left) < xEnd && static_cast(right) >= xBegin && + static_cast(right) < xEnd && static_cast(top) >= yBegin && static_cast(top) < yEnd && + static_cast(bottom) >= yBegin && static_cast(bottom) < yEnd); + } while (!stop); + + k_indices.resize(results.size()); + k_sqr_distances.resize(results.size()); + size_t idx = results.size() - 1; + while (!results.empty()) { + k_indices[idx] = results.top().index; + k_sqr_distances[idx] = results.top().distance; + results.pop(); + --idx; + } + + return (static_cast(k_indices.size())); +} + +//////////////////////////////////////////////////////////////////////////////////////////// +template +void pcl::search::OrganizedNeighbor2::getProjectedRadiusSearchBox(const PointT& point, float squared_radius, + unsigned& minX, unsigned& maxX, + unsigned& minY, unsigned& maxY) const { + const Eigen::Vector3f queryvec(point.x, point.y, point.z); + const Eigen::Vector2f q = projectPoint(queryvec); + + float a = squared_radius * KR_KRT_.coeff(8) - q[2] * q[2]; + float b = squared_radius * KR_KRT_.coeff(7) - q[1] * q[2]; + float c = squared_radius * KR_KRT_.coeff(4) - q[1] * q[1]; + int min, max; + // a and c are multiplied by two already => - 4ac -> - ac + float det = b * b - a * c; + if (det < 0) { + minY = 0; + maxY = input_->height - 1; + } else { + float y1 = static_cast((b - sqrt(det)) / a); + float y2 = static_cast((b + sqrt(det)) / a); + + min = std::min(static_cast(floor(y1)), static_cast(floor(y2))); + max = std::max(static_cast(ceil(y1)), static_cast(ceil(y2))); + minY = static_cast(std::min(static_cast(input_->height) - 1, std::max(0, min))); + maxY = static_cast(std::max(std::min(static_cast(input_->height) - 1, max), 0)); + } + + b = squared_radius * KR_KRT_.coeff(6) - q[0] * q[2]; + c = squared_radius * KR_KRT_.coeff(0) - q[0] * q[0]; + + det = b * b - a * c; + if (det < 0) { + minX = 0; + maxX = input_->width - 1; + } else { + float x1 = static_cast((b - sqrt(det)) / a); + float x2 = static_cast((b + sqrt(det)) / a); + + min = std::min(static_cast(floor(x1)), static_cast(floor(x2))); + max = std::max(static_cast(ceil(x1)), static_cast(ceil(x2))); + minX = static_cast(std::min(static_cast(input_->width) - 1, std::max(0, min))); + maxX = static_cast(std::max(std::min(static_cast(input_->width) - 1, max), 0)); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +Eigen::Vector2f pcl::search::OrganizedNeighbor2::projectPoint(const Eigen::Vector3f& point) const { + Eigen::Vector3f projected = intrinsics_matrix_ * point; + Eigen::Vector2f q; + q.x() = projected[0] / projected[2]; + q.y() = projected[1] / projected[2]; + return q; +} +#define PCL_INSTANTIATE_OrganizedNeighbor2(T) template class PCL_EXPORTS pcl::search::OrganizedNeighbor2; + +#endif // POINT_CLOUD_COMMON_ORGANIZED_NEIGHBOR2_IMPL_H_ diff --git a/localization/point_cloud_common/include/point_cloud_common/parameter_reader.h b/localization/point_cloud_common/include/point_cloud_common/parameter_reader.h new file mode 100644 index 0000000000..81b14cda7c --- /dev/null +++ b/localization/point_cloud_common/include/point_cloud_common/parameter_reader.h @@ -0,0 +1,30 @@ +/* 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. + */ +#ifndef POINT_CLOUD_COMMON_PARAMETER_READER_H_ +#define POINT_CLOUD_COMMON_PARAMETER_READER_H_ + +#include +#include +#include + +namespace point_cloud_common { +void LoadPointToPlaneICPParams(config_reader::ConfigReader& config, PointToPlaneICPParams& params); +void LoadPointCloudWithKnownCorrespondencesAlignerParams(config_reader::ConfigReader& config, + PointCloudWithKnownCorrespondencesAlignerParams& params); +} // namespace point_cloud_common +#endif // POINT_CLOUD_COMMON_PARAMETER_READER_H_ diff --git a/localization/point_cloud_common/include/point_cloud_common/point_cloud_with_known_correspondences_aligner.h b/localization/point_cloud_common/include/point_cloud_common/point_cloud_with_known_correspondences_aligner.h new file mode 100644 index 0000000000..71acbc64fe --- /dev/null +++ b/localization/point_cloud_common/include/point_cloud_common/point_cloud_with_known_correspondences_aligner.h @@ -0,0 +1,62 @@ +/* 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. + */ +#ifndef POINT_CLOUD_COMMON_POINT_CLOUD_WITH_KNOWN_CORRESPONDENCES_ALIGNER_H_ +#define POINT_CLOUD_COMMON_POINT_CLOUD_WITH_KNOWN_CORRESPONDENCES_ALIGNER_H_ + +#include +#include + +#include + +#include +#include + +#include + +#include + +namespace point_cloud_common { +class PointCloudWithKnownCorrespondencesAligner { + public: + explicit PointCloudWithKnownCorrespondencesAligner(const PointCloudWithKnownCorrespondencesAlignerParams& params); + + boost::optional Align( + const std::vector& source_points, const std::vector& target_points, + const Eigen::Isometry3d& initial_target_T_source_estimate, + const boost::optional&> source_normals = boost::none, + const boost::optional&> target_normals = boost::none) const; + + boost::optional ComputeRelativeTransform( + const std::vector& source_points, const std::vector& target_points, + const boost::optional&> source_normals = boost::none, + const boost::optional&> target_normals = boost::none, + const Eigen::Isometry3d& initial_target_T_source_estimate = Eigen::Isometry3d::Identity()) const; + + boost::optional ComputeRelativeTransform( + const std::vector& source_points, const std::vector& target_points, + const Eigen::Isometry3d& initial_target_T_source_estimate = Eigen::Isometry3d::Identity()) const; + + private: + boost::optional Covariance(const double* const target_T_source_data, + ceres::Problem& problem) const; + + PointCloudWithKnownCorrespondencesAlignerParams params_; +}; +} // namespace point_cloud_common + +#endif // POINT_CLOUD_COMMON_POINT_CLOUD_WITH_KNOWN_CORRESPONDENCES_ALIGNER_H_ diff --git a/localization/point_cloud_common/include/point_cloud_common/point_cloud_with_known_correspondences_aligner_params.h b/localization/point_cloud_common/include/point_cloud_common/point_cloud_with_known_correspondences_aligner_params.h new file mode 100644 index 0000000000..5cd845af1d --- /dev/null +++ b/localization/point_cloud_common/include/point_cloud_common/point_cloud_with_known_correspondences_aligner_params.h @@ -0,0 +1,36 @@ +/* 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. + */ +#ifndef POINT_CLOUD_COMMON_POINT_CLOUD_WITH_KNOWN_CORRESPONDENCES_ALIGNER_PARAMS_H_ +#define POINT_CLOUD_COMMON_POINT_CLOUD_WITH_KNOWN_CORRESPONDENCES_ALIGNER_PARAMS_H_ + +namespace point_cloud_common { +struct PointCloudWithKnownCorrespondencesAlignerParams { + int max_num_iterations; + double function_tolerance; + int max_num_matches; + double normal_search_radius; + bool use_umeyama_initial_guess; + // TODO(rsoussan): these are exclusive, enforce this some how + bool use_single_iteration_umeyama; + bool use_point_to_plane_cost; + bool use_symmetric_point_to_plane_cost; + bool verbose_optimization; +}; +} // namespace point_cloud_common + +#endif // POINT_CLOUD_COMMON_POINT_CLOUD_WITH_KNOWN_CORRESPONDENCES_ALIGNER_PARAMS_H_ diff --git a/localization/point_cloud_common/include/point_cloud_common/point_to_plane_icp.h b/localization/point_cloud_common/include/point_cloud_common/point_to_plane_icp.h new file mode 100644 index 0000000000..88e36faba7 --- /dev/null +++ b/localization/point_cloud_common/include/point_cloud_common/point_to_plane_icp.h @@ -0,0 +1,222 @@ +/* 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. + */ +#ifndef POINT_CLOUD_COMMON_POINT_TO_PLANE_ICP_H_ +#define POINT_CLOUD_COMMON_POINT_TO_PLANE_ICP_H_ + +#include +#include +#include +#include +#include +#include +#include + +#include + +// TODO(rsoussan): Switch back to this when PCL bug is fixed +// #include +#include +// TODO(rsoussan): Remove this when PCL upgraded +#include +#include +#include +#include +#include + +#include + +namespace point_cloud_common { +template +class PointToPlaneICP { + public: + explicit PointToPlaneICP(const PointToPlaneICPParams& params); + boost::optional ComputeRelativeTransform( + const typename pcl::PointCloud::Ptr source_cloud_with_normals, + const typename pcl::PointCloud::Ptr target_cloud_with_normals, + const Eigen::Isometry3d& initial_target_T_source_estimate = Eigen::Isometry3d::Identity()); + const boost::optional& correspondences() const; + + private: + boost::optional RunPointToPlaneICP( + const typename pcl::PointCloud::Ptr source_cloud_with_normals, + const typename pcl::PointCloud::Ptr target_cloud_with_normals, + const Eigen::Isometry3d& initial_target_T_source_estimate = Eigen::Isometry3d::Identity()); + boost::optional RunCoarseToFinePointToPlaneICP( + const typename pcl::PointCloud::Ptr source_cloud_with_normals, + const typename pcl::PointCloud::Ptr target_cloud_with_normals, + const Eigen::Isometry3d& initial_target_T_source_estimate = Eigen::Isometry3d::Identity()); + boost::optional RunDownsampledPointToPlaneICP( + const typename pcl::PointCloud::Ptr source_cloud_with_normals, + const typename pcl::PointCloud::Ptr target_cloud_with_normals, const double leaf_size, + const Eigen::Isometry3d& initial_target_T_source_estimate = Eigen::Isometry3d::Identity()); + void SaveCorrespondences(const pcl::IterativeClosestPointWithNormals& icp, + const typename pcl::PointCloud::Ptr source_cloud, + const typename pcl::PointCloud::Ptr source_cloud_transformed); + boost::optional correspondences_; + PointToPlaneICPParams params_; +}; + +// Implemenation +template +PointToPlaneICP::PointToPlaneICP(const PointToPlaneICPParams& params) : params_(params) {} + +template +boost::optional PointToPlaneICP::ComputeRelativeTransform( + const typename pcl::PointCloud::Ptr source_cloud_with_normals, + const typename pcl::PointCloud::Ptr target_cloud_with_normals, + const Eigen::Isometry3d& initial_target_T_source_estimate) { + if (params_.coarse_to_fine) { + return RunCoarseToFinePointToPlaneICP(source_cloud_with_normals, target_cloud_with_normals, + initial_target_T_source_estimate); + } else { + return RunPointToPlaneICP(source_cloud_with_normals, target_cloud_with_normals, initial_target_T_source_estimate); + } +} + +template +boost::optional PointToPlaneICP::RunPointToPlaneICP( + const typename pcl::PointCloud::Ptr source_cloud_with_normals, + const typename pcl::PointCloud::Ptr target_cloud_with_normals, + const Eigen::Isometry3d& initial_target_T_source_estimate) { + pcl::IterativeClosestPointWithNormals icp; + + if (params_.symmetric_objective) { + auto symmetric_transformation_estimation = + boost::make_shared>(); + symmetric_transformation_estimation->setEnforceSameDirectionNormals(params_.enforce_same_direction_normals); + icp.transformation_estimation_ = symmetric_transformation_estimation; + } + + if (params_.correspondence_rejector_surface_normal) { + typename pcl::registration::CorrespondenceRejectorSurfaceNormal2::Ptr + correspondence_rejector_surface_normal(new pcl::registration::CorrespondenceRejectorSurfaceNormal2()); + correspondence_rejector_surface_normal->initializeDataContainer(); + correspondence_rejector_surface_normal->setThreshold(params_.correspondence_rejector_surface_normal_threshold); + correspondence_rejector_surface_normal->setInputNormals(source_cloud_with_normals); + correspondence_rejector_surface_normal->setTargetNormals(target_cloud_with_normals); + icp.addCorrespondenceRejector(correspondence_rejector_surface_normal); + } + + if (params_.correspondence_rejector_median_distance) { + pcl::registration::CorrespondenceRejectorMedianDistance::Ptr correspondence_rejector_median_distance( + new pcl::registration::CorrespondenceRejectorMedianDistance()); + correspondence_rejector_median_distance->setMedianFactor(params_.correspondence_rejector_median_distance_factor); + icp.addCorrespondenceRejector(correspondence_rejector_median_distance); + } + + icp.setInputSource(source_cloud_with_normals); + icp.setInputTarget(target_cloud_with_normals); + icp.setMaximumIterations(params_.max_iterations); + typename pcl::PointCloud::Ptr result(new pcl::PointCloud); + icp.align(*result, initial_target_T_source_estimate.matrix().cast()); + + if (!icp.hasConverged()) { + LogError("Icp: Failed to converge."); + return boost::none; + } + + if (params_.use_fitness_threshold_rejection) { + const double fitness_score = icp.getFitnessScore(); + if (fitness_score > params_.fitness_threshold) { + LogError("Icp: Fitness score too large: " << fitness_score << "."); + return boost::none; + } + } + + const Eigen::Isometry3d estimated_target_T_source( + Eigen::Isometry3f(icp.getFinalTransformation().matrix()).cast()); + SaveCorrespondences(icp, source_cloud_with_normals, result); + const auto covariance = PointToPlaneCovariance(correspondences_->source_points, correspondences_->target_normals, + estimated_target_T_source); + if (!covariance) { + LogError("Icp: Failed to get covariance."); + return boost::none; + } + return localization_common::PoseWithCovariance(estimated_target_T_source, *covariance); +} + +template +boost::optional PointToPlaneICP::RunDownsampledPointToPlaneICP( + const typename pcl::PointCloud::Ptr source_cloud_with_normals, + const typename pcl::PointCloud::Ptr target_cloud_with_normals, const double leaf_size, + const Eigen::Isometry3d& initial_target_T_source_estimate) { + typename pcl::PointCloud::Ptr downsampled_source_cloud_with_normals = + DownsamplePointCloud(source_cloud_with_normals, leaf_size); + typename pcl::PointCloud::Ptr downsampled_target_cloud_with_normals = + DownsamplePointCloud(target_cloud_with_normals, leaf_size); + return RunPointToPlaneICP(downsampled_source_cloud_with_normals, downsampled_target_cloud_with_normals, + initial_target_T_source_estimate); +} + +template +boost::optional PointToPlaneICP::RunCoarseToFinePointToPlaneICP( + const typename pcl::PointCloud::Ptr source_cloud_with_normals, + const typename pcl::PointCloud::Ptr target_cloud_with_normals, + const Eigen::Isometry3d& initial_target_T_source_estimate) { + boost::optional estimated_target_T_source = + localization_common::PoseWithCovariance(initial_target_T_source_estimate, Eigen::Matrix()); + for (int i = 0; i < params_.num_coarse_to_fine_levels; ++i) { + // Final iteration and no downsampling enabled for last iteration + if (i == params_.num_coarse_to_fine_levels - 1 && !params_.downsample_last_coarse_to_fine_iteration) { + estimated_target_T_source = + RunPointToPlaneICP(source_cloud_with_normals, target_cloud_with_normals, estimated_target_T_source->pose); + } else { // Downsampled for other iterations + const double leaf_size = + static_cast(params_.num_coarse_to_fine_levels - i) * params_.coarse_to_fine_final_leaf_size; + estimated_target_T_source = RunDownsampledPointToPlaneICP(source_cloud_with_normals, target_cloud_with_normals, + leaf_size, estimated_target_T_source->pose); + } + if (!estimated_target_T_source) { + LogWarning("RunCoarseToFinePointToPlaneICP: Failed to get relative transform."); + return boost::none; + } + } + return estimated_target_T_source; +} + +template +void PointToPlaneICP::SaveCorrespondences( + const pcl::IterativeClosestPointWithNormals& icp, + const typename pcl::PointCloud::Ptr source_cloud, + const typename pcl::PointCloud::Ptr source_cloud_transformed) { + icp.correspondence_estimation_->setInputSource(source_cloud_transformed); + pcl::Correspondences pcl_correspondences; + // Assumes normals for input source aren't needed and there are no correspondence rejectors added to ICP object + icp.correspondence_estimation_->determineCorrespondences(pcl_correspondences, icp.corr_dist_threshold_); + const auto& target_cloud = icp.target_; + FilterCorrespondences(*source_cloud, *target_cloud, pcl_correspondences); + std::vector source_points; + std::vector target_points; + std::vector target_normals; + for (const auto& correspondence : pcl_correspondences) { + const auto& pcl_source_point = (*source_cloud)[correspondence.index_query]; + const auto& pcl_target_point = (*target_cloud)[correspondence.index_match]; + source_points.emplace_back(Vector3d(pcl_source_point)); + target_points.emplace_back(Vector3d(pcl_target_point)); + target_normals.emplace_back(NormalVector3d(pcl_target_point)); + } + correspondences_ = ICPCorrespondences(source_points, target_points, target_normals); +} + +template +const boost::optional& PointToPlaneICP::correspondences() const { + return correspondences_; +} +} // namespace point_cloud_common + +#endif // POINT_CLOUD_COMMON_POINT_TO_PLANE_ICP_H_ diff --git a/localization/point_cloud_common/include/point_cloud_common/point_to_plane_icp_params.h b/localization/point_cloud_common/include/point_cloud_common/point_to_plane_icp_params.h new file mode 100644 index 0000000000..fcb1c5b90c --- /dev/null +++ b/localization/point_cloud_common/include/point_cloud_common/point_to_plane_icp_params.h @@ -0,0 +1,41 @@ +/* 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. + */ +#ifndef POINT_CLOUD_COMMON_POINT_TO_PLANE_ICP_PARAMS_H_ +#define POINT_CLOUD_COMMON_POINT_TO_PLANE_ICP_PARAMS_H_ + +namespace point_cloud_common { +struct PointToPlaneICPParams { + double search_radius; + bool use_fitness_threshold_rejection; + double fitness_threshold; + int max_iterations; + bool symmetric_objective; + bool enforce_same_direction_normals; + bool correspondence_rejector_surface_normal; + double correspondence_rejector_surface_normal_threshold; + bool correspondence_rejector_median_distance; + double correspondence_rejector_median_distance_factor; + // Coarse to fine options + bool coarse_to_fine; + int num_coarse_to_fine_levels; + double coarse_to_fine_final_leaf_size; + bool downsample_last_coarse_to_fine_iteration; +}; +} // namespace point_cloud_common + +#endif // POINT_CLOUD_COMMON_POINT_TO_PLANE_ICP_PARAMS_H_ diff --git a/localization/point_cloud_common/include/point_cloud_common/test_utilities.h b/localization/point_cloud_common/include/point_cloud_common/test_utilities.h new file mode 100644 index 0000000000..2e5af15494 --- /dev/null +++ b/localization/point_cloud_common/include/point_cloud_common/test_utilities.h @@ -0,0 +1,82 @@ +/* Copyright (c) 2017, United S/ates 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. + */ +#ifndef POINT_CLOUD_COMMON_TEST_UTILITIES_H_ +#define POINT_CLOUD_COMMON_TEST_UTILITIES_H_ + +#include +#include +#include + +#include + +#include +#include + +#include +#include + +namespace point_cloud_common { +std::vector RandomPoints(const int num_points); + +// Assumes width and height vecs are normalized and define the plane attached to point +std::vector PlanePoints(const Eigen::Vector3d& point, const Eigen::Vector3d& width_vec, + const Eigen::Vector3d& height_vec, const double width, const double height, + const int num_width_points, const double num_height_points); + +std::pair, std::vector> RandomPointsWithNormals(const int num_points); + +// Returns points on three unqiue planes covering half of a cube +std::pair, std::vector> CubicPoints(); + +pcl::PointXYZ PCLPoint(const Eigen::Vector3d& point); + +pcl::PointNormal PCLPointNormal(const Eigen::Vector3d& point, const Eigen::Vector3d& normal); + +template +typename pcl::PointCloud::Ptr PointCloud(const std::vector& points); + +template +PointType PCLPoint(const Eigen::Vector3d& point); + +pcl::PointCloud::Ptr PointCloudWithNormals(const std::vector& points, + const std::vector& normals); + +PointToPlaneICPParams DefaultPointToPlaneICPParams(); + +PointCloudWithKnownCorrespondencesAlignerParams DefaultPointCloudWithKnownCorrespondencesAlignerParams(); + +// Implementation +template +typename pcl::PointCloud::Ptr PointCloud(const std::vector& points) { + typename pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); + for (const auto& point : points) { + cloud->points.emplace_back(PCLPoint(point)); + } + return cloud; +} + +template +PointType PCLPoint(const Eigen::Vector3d& point) { + PointType pcl_point; + pcl_point.x = point.x(); + pcl_point.y = point.y(); + pcl_point.z = point.z(); + return pcl_point; +} +} // namespace point_cloud_common +#endif // POINT_CLOUD_COMMON_TEST_UTILITIES_H_ diff --git a/localization/point_cloud_common/include/point_cloud_common/transformation_estimation_symmetric_point_to_plane_lls.h b/localization/point_cloud_common/include/point_cloud_common/transformation_estimation_symmetric_point_to_plane_lls.h new file mode 100644 index 0000000000..0211457128 --- /dev/null +++ b/localization/point_cloud_common/include/point_cloud_common/transformation_estimation_symmetric_point_to_plane_lls.h @@ -0,0 +1,154 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2019-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ +// TODO(rsoussan): Remove this file from repo when PCL updated to version >= 1.10 + +#ifndef POINT_CLOUD_COMMON_TRANSFORMATION_ESTIMATION_SYMMETRIC_POINT_TO_PLANE_LLS_H_ +#define POINT_CLOUD_COMMON_TRANSFORMATION_ESTIMATION_SYMMETRIC_POINT_TO_PLANE_LLS_H_ + +#include +#include +#include + +#include + +namespace pcl { +namespace registration { +/** \brief @b TransformationEstimationSymmetricPointToPlaneLLS implements a Linear Least Squares (LLS) approximation + * for minimizing the symmetric point-to-plane distance between two clouds of corresponding points with normals. + * + * For additional details, see + * "Linear Least-Squares Optimization for Point-to-Plane ICP Surface Registration", Kok-Lim Low, 2004 + * "A Symmetric Objective Function for ICP", Szymon Rusinkiewicz, 2019 + * + * \note The class is templated on the source and target point types as well as on the output scalar of the + * transformation matrix (i.e., float or double). Default: float. + * \author Matthew Cong + * \ingroup registration + */ +template +class TransformationEstimationSymmetricPointToPlaneLLS2 + : public TransformationEstimation { + public: + using Ptr = boost::shared_ptr >; + using ConstPtr = + boost::shared_ptr >; + + using Matrix4 = typename TransformationEstimation::Matrix4; + using Vector6 = Eigen::Matrix; + + TransformationEstimationSymmetricPointToPlaneLLS2() : enforce_same_direction_normals_(true) {} + ~TransformationEstimationSymmetricPointToPlaneLLS2() {} + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD. + * \param[in] cloud_src the source point cloud dataset + * \param[in] cloud_tgt the target point cloud dataset + * \param[out] transformation_matrix the resultant transformation matrix + */ + inline void estimateRigidTransformation(const pcl::PointCloud& cloud_src, + const pcl::PointCloud& cloud_tgt, + Matrix4& transformation_matrix) const override; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD. + * \param[in] cloud_src the source point cloud dataset + * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src + * \param[in] cloud_tgt the target point cloud dataset + * \param[out] transformation_matrix the resultant transformation matrix + */ + inline void estimateRigidTransformation(const pcl::PointCloud& cloud_src, + const std::vector& indices_src, + const pcl::PointCloud& cloud_tgt, + Matrix4& transformation_matrix) const override; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD. + * \param[in] cloud_src the source point cloud dataset + * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src + * \param[in] cloud_tgt the target point cloud dataset + * \param[in] indices_tgt the vector of indices describing the correspondences of the interest points from \a + * indices_src \param[out] transformation_matrix the resultant transformation matrix + */ + inline void estimateRigidTransformation(const pcl::PointCloud& cloud_src, + const std::vector& indices_src, + const pcl::PointCloud& cloud_tgt, + const std::vector& indices_tgt, + Matrix4& transformation_matrix) const override; + + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD. + * \param[in] cloud_src the source point cloud dataset + * \param[in] cloud_tgt the target point cloud dataset + * \param[in] correspondences the vector of correspondences between source and target point cloud + * \param[out] transformation_matrix the resultant transformation matrix + */ + inline void estimateRigidTransformation(const pcl::PointCloud& cloud_src, + const pcl::PointCloud& cloud_tgt, + const pcl::Correspondences& correspondences, + Matrix4& transformation_matrix) const override; + + /** \brief Set whether or not to negate source or target normals on a per-point basis such that they point in the same + * direction. \param[in] enforce_same_direction_normals whether to negate source or target normals on a per-point + * basis such that they point in the same direction. + */ + inline void setEnforceSameDirectionNormals(bool enforce_same_direction_normals); + + /** \brief Obtain whether source or target normals are negated on a per-point basis such that they point in the same + * direction or not */ + inline bool getEnforceSameDirectionNormals(); + + protected: + /** \brief Estimate a rigid rotation transformation between a source and a target + * \param[in] source_it an iterator over the source point cloud dataset + * \param[in] target_it an iterator over the target point cloud dataset + * \param[out] transformation_matrix the resultant transformation matrix + */ + void estimateRigidTransformation(ConstCloudIterator& source_it, + ConstCloudIterator& target_it, Matrix4& transformation_matrix) const; + + /** \brief Construct a 4 by 4 transformation matrix from the provided rotation and translation. + * \param[in] parameters (alpha, beta, gamma, tx, ty, tz) specifying rotation about the x, y, and z-axis and + * translation along the the x, y, and z-axis respectively \param[out] transformation_matrix the resultant + * transformation matrix + */ + inline void constructTransformationMatrix(const Vector6& parameters, Matrix4& transformation_matrix) const; + + /** \brief Whether or not to negate source and/or target normals such that they point in the same direction */ + bool enforce_same_direction_normals_; +}; +} // namespace registration +} // namespace pcl + +#include // NOLINT + +#endif // POINT_CLOUD_COMMON_TRANSFORMATION_ESTIMATION_SYMMETRIC_POINT_TO_PLANE_LLS_H_ diff --git a/localization/point_cloud_common/include/point_cloud_common/transformation_estimation_symmetric_point_to_plane_lls_impl.h b/localization/point_cloud_common/include/point_cloud_common/transformation_estimation_symmetric_point_to_plane_lls_impl.h new file mode 100644 index 0000000000..161437d848 --- /dev/null +++ b/localization/point_cloud_common/include/point_cloud_common/transformation_estimation_symmetric_point_to_plane_lls_impl.h @@ -0,0 +1,199 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2019-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +// TODO(rsoussan): Remove this file from repo when PCL updated to version >= 1.10 +#ifndef POINT_CLOUD_COMMON_TRANSFORMATION_ESTIMATION_SYMMETRIC_POINT_TO_PLANE_LLS_IMPL_H_ +#define POINT_CLOUD_COMMON_TRANSFORMATION_ESTIMATION_SYMMETRIC_POINT_TO_PLANE_LLS_IMPL_H_ + +#include + +#include + +namespace pcl { + +namespace registration { + +template +inline void +TransformationEstimationSymmetricPointToPlaneLLS2::estimateRigidTransformation( + const pcl::PointCloud& cloud_src, const pcl::PointCloud& cloud_tgt, + Matrix4& transformation_matrix) const { + const auto nr_points = cloud_src.size(); + if (cloud_tgt.size() != nr_points) { + PCL_ERROR( + "[pcl::TransformationEstimationSymmetricPointToPlaneLLS::" + "estimateRigidTransformation] Number or points in source (%zu) differs " + "from target (%zu)!\n", + static_cast(nr_points), static_cast(cloud_tgt.size())); + return; + } + + ConstCloudIterator source_it(cloud_src); + ConstCloudIterator target_it(cloud_tgt); + estimateRigidTransformation(source_it, target_it, transformation_matrix); +} + +template +void TransformationEstimationSymmetricPointToPlaneLLS2::estimateRigidTransformation( + const pcl::PointCloud& cloud_src, const std::vector& indices_src, + const pcl::PointCloud& cloud_tgt, Matrix4& transformation_matrix) const { + const auto nr_points = indices_src.size(); + if (cloud_tgt.size() != nr_points) { + PCL_ERROR( + "[pcl::TransformationEstimationSymmetricPointToPlaneLLS::" + "estimateRigidTransformation] Number or points in source (%zu) differs " + "than target (%zu)!\n", + indices_src.size(), static_cast(cloud_tgt.size())); + return; + } + + ConstCloudIterator source_it(cloud_src, indices_src); + ConstCloudIterator target_it(cloud_tgt); + estimateRigidTransformation(source_it, target_it, transformation_matrix); +} + +template +inline void +TransformationEstimationSymmetricPointToPlaneLLS2::estimateRigidTransformation( + const pcl::PointCloud& cloud_src, const std::vector& indices_src, + const pcl::PointCloud& cloud_tgt, const std::vector& indices_tgt, + Matrix4& transformation_matrix) const { + const auto nr_points = indices_src.size(); + if (indices_tgt.size() != nr_points) { + PCL_ERROR( + "[pcl::TransformationEstimationSymmetricPointToPlaneLLS::" + "estimateRigidTransformation] Number or points in source (%zu) differs " + "than target (%zu)!\n", + indices_src.size(), indices_tgt.size()); + return; + } + + ConstCloudIterator source_it(cloud_src, indices_src); + ConstCloudIterator target_it(cloud_tgt, indices_tgt); + estimateRigidTransformation(source_it, target_it, transformation_matrix); +} + +template +inline void +TransformationEstimationSymmetricPointToPlaneLLS2::estimateRigidTransformation( + const pcl::PointCloud& cloud_src, const pcl::PointCloud& cloud_tgt, + const pcl::Correspondences& correspondences, Matrix4& transformation_matrix) const { + ConstCloudIterator source_it(cloud_src, correspondences, true); + ConstCloudIterator target_it(cloud_tgt, correspondences, false); + estimateRigidTransformation(source_it, target_it, transformation_matrix); +} + +template +inline void +TransformationEstimationSymmetricPointToPlaneLLS2::constructTransformationMatrix( + const Vector6& parameters, Matrix4& transformation_matrix) const { + // Construct the transformation matrix from rotation and translation + const Eigen::AngleAxis rotation_z(parameters(2), Eigen::Matrix::UnitZ()); + const Eigen::AngleAxis rotation_y(parameters(1), Eigen::Matrix::UnitY()); + const Eigen::AngleAxis rotation_x(parameters(0), Eigen::Matrix::UnitX()); + const Eigen::Translation translation(parameters(3), parameters(4), parameters(5)); + const Eigen::Transform transform = + rotation_z * rotation_y * rotation_x * translation * rotation_z * rotation_y * rotation_x; + transformation_matrix = transform.matrix(); +} + +template +inline void +TransformationEstimationSymmetricPointToPlaneLLS2::estimateRigidTransformation( + ConstCloudIterator& source_it, ConstCloudIterator& target_it, + Matrix4& transformation_matrix) const { + using Matrix6 = Eigen::Matrix; + using Vector3 = Eigen::Matrix; + + Matrix6 ATA; + Vector6 ATb; + ATA.setZero(); + ATb.setZero(); + auto M = ATA.template selfadjointView(); + + // Approximate as a linear least squares problem + source_it.reset(); + target_it.reset(); + for (; source_it.isValid() && target_it.isValid(); ++source_it, ++target_it) { + const Vector3 p(source_it->x, source_it->y, source_it->z); + const Vector3 q(target_it->x, target_it->y, target_it->z); + const Vector3 n1(source_it->getNormalVector3fMap().template cast()); + const Vector3 n2(target_it->getNormalVector3fMap().template cast()); + Vector3 n; + if (enforce_same_direction_normals_) { + if (n1.dot(n2) >= 0.) + n = n1 + n2; + else + n = n1 - n2; + } else { + n = n1 + n2; + } + + if (!p.array().isFinite().all() || !q.array().isFinite().all() || !n.array().isFinite().all()) { + continue; + } + + Vector6 v; + v << (p + q).cross(n), n; + M.rankUpdate(v); + + ATb += v * (q - p).dot(n); + } + + // Solve A*x = b + const Vector6 x = M.ldlt().solve(ATb); + + // Construct the transformation matrix from x + constructTransformationMatrix(x, transformation_matrix); +} + +template +inline void +TransformationEstimationSymmetricPointToPlaneLLS2::setEnforceSameDirectionNormals( + bool enforce_same_direction_normals) { + enforce_same_direction_normals_ = enforce_same_direction_normals; +} + +template +inline bool +TransformationEstimationSymmetricPointToPlaneLLS2::getEnforceSameDirectionNormals() { + return enforce_same_direction_normals_; +} + +} // namespace registration +} // namespace pcl +#endif // POINT_CLOUD_COMMON_TRANSFORMATION_ESTIMATION_SYMMETRIC_POINT_TO_PLANE_LLS_IMPL_H_ diff --git a/localization/point_cloud_common/include/point_cloud_common/utilities.h b/localization/point_cloud_common/include/point_cloud_common/utilities.h new file mode 100644 index 0000000000..a84a343239 --- /dev/null +++ b/localization/point_cloud_common/include/point_cloud_common/utilities.h @@ -0,0 +1,387 @@ +/* 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. + */ +#ifndef POINT_CLOUD_COMMON_UTILITIES_H_ +#define POINT_CLOUD_COMMON_UTILITIES_H_ + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace point_cloud_common { +template +void EstimateNormals(const typename pcl::PointCloud::Ptr cloud, const double search_radius, + pcl::PointCloud& cloud_with_normals); + +template +void EstimateOrganizedNormals( + const typename pcl::PointCloud::Ptr cloud, + const typename pcl::IntegralImageNormalEstimation::NormalEstimationMethod method, + const bool depth_dependent_smoothing, const double max_depth_change_factor, const double normal_smoothing_size, + pcl::PointCloud& cloud_with_normals); + +template +void NormalSpaceSubsampling(typename pcl::PointCloud::Ptr cloud, const int bins_per_axis, + const int num_samples); + +Eigen::Matrix4f RansacIA(const pcl::PointCloud::Ptr source_cloud, + const pcl::PointCloud::Ptr target_cloud); + +pcl::PointCloud::Ptr EstimateHistogramFeatures( + const pcl::PointCloud::Ptr cloud_with_normals); + +Eigen::Matrix PointToPlaneJacobian(const gtsam::Point3& source_point, const gtsam::Vector3& normal, + const gtsam::Pose3& target_T_source); + +Eigen::Matrix PointToPointJacobian(const gtsam::Point3& source_point, + const gtsam::Pose3& target_T_source); + +Eigen::Isometry3d RelativeTransformUmeyama(const std::vector& source_points, + const std::vector& target_points); + +template +boost::optional GetNormal(const Eigen::Vector3d& point, const pcl::PointCloud& cloud, + const pcl::search::KdTree& kdtree, + const double search_radius = 0.03); + +template +bool computePointNormal(const pcl::PointCloud& cloud, const std::vector& indices, float& normal_x, + float& normal_y, float& normal_z, float& curvature); + +boost::optional> PointToPointCovariance(const std::vector& source_points, + const Eigen::Isometry3d& target_T_source); + +boost::optional> PointToPlaneCovariance(const std::vector& source_points, + const std::vector& target_normals, + const Eigen::Isometry3d& target_T_source); + +template +Eigen::Vector3d Vector3d(const PointType& point); + +template +Eigen::Vector3d NormalVector3d(const PointType& point_with_normal); + +pcl::PointXYZI Interpolate(const pcl::PointXYZI& point_a, const pcl::PointXYZI& point_b, const double alpha); + +template +bool ValidPoint(const PointType& point) = delete; + +template <> +bool ValidPoint(const pcl::PointXYZ& point); + +template <> +bool ValidPoint(const pcl::PointXYZI& point); + +template <> +bool ValidPoint(const pcl::PointNormal& point); + +template <> +bool ValidPoint(const pcl::PointXYZINormal& point); + +template +bool ApproxZero(const Type& point, const double epsilon = 1e-5) { + return std::abs(point) <= epsilon; +} + +template +bool NonzeroPointXYZ(const PointXYZType& point); + +template +bool ValidPointXYZ(const PointXYZType& point); + +template +bool ValidNormal(const PointNormalType& point); + +template +bool ValidIntensity(const PointIntensityType& point); + +template +void RemoveInvalidAndZeroPoints(pcl::PointCloud& cloud); + +// Some pcl functions expect invalid points to be labeled with nans while +// some sensors label invalid points with all zeros. +template +void ReplaceZerosWithNans(typename pcl::PointCloud& cloud); + +template +typename pcl::PointCloud::Ptr DownsamplePointCloud(const typename pcl::PointCloud::Ptr cloud, + const double leaf_size); + +template +typename pcl::PointCloud::Ptr BilateralFilterOrganizedCloud( + const typename pcl::PointCloud::Ptr cloud, const double sigma_s, const double sigma_r); +template +void FilterCorrespondences(const typename pcl::PointCloud& input_cloud, + const typename pcl::PointCloud& target_cloud, + pcl::Correspondences& correspondences); + +template +typename pcl::PointCloud::Ptr FilteredPointCloud( + const typename pcl::PointCloud::Ptr unfiltered_cloud); + +template +typename pcl::PointCloud::Ptr FilteredPointCloudWithNormals( + const typename pcl::PointCloud::Ptr unfiltered_cloud, const double search_radius); + +// Implementation +template +bool NonzeroPointXYZ(const PointXYZType& point) { + bool nonzero_point = !ApproxZero(point.x) || !ApproxZero(point.y) || !ApproxZero(point.z); + return nonzero_point; +} + +template +bool ValidPointXYZ(const PointXYZType& point) { + const bool finite_point = pcl_isfinite(point.x) && pcl_isfinite(point.y) && pcl_isfinite(point.z); + const bool nonzero_point = NonzeroPointXYZ(point); + return finite_point && nonzero_point; +} + +template +bool ValidNormal(const PointNormalType& point) { + const bool finite_normal = + pcl_isfinite(point.normal_x) && pcl_isfinite(point.normal_y) && pcl_isfinite(point.normal_z); + const bool nonzero_normal = !ApproxZero(point.normal_x) || !ApproxZero(point.normal_y) || !ApproxZero(point.normal_z); + return finite_normal && nonzero_normal; +} + +template +bool ValidIntensity(const PointIntensityType& point) { + return pcl_isfinite(point.intensity); +} + +template +void RemoveInvalidAndZeroPoints(pcl::PointCloud& cloud) { + size_t new_index = 0; + for (const auto& point : cloud.points) { + const bool valid_point = ValidPoint(point); + if (!valid_point) continue; + cloud.points[new_index++] = point; + } + if (new_index != cloud.points.size()) { + cloud.points.resize(new_index); + } + + cloud.height = 1; + cloud.width = static_cast(new_index); + cloud.is_dense = true; +} + +template +void ReplaceZerosWithNans(typename pcl::PointCloud& cloud) { + for (auto& point : cloud.points) { + const bool nonzero_point = NonzeroPointXYZ(point); + if (nonzero_point) { + continue; + } else { + point.x = std::numeric_limits::quiet_NaN(); + point.y = std::numeric_limits::quiet_NaN(); + point.z = std::numeric_limits::quiet_NaN(); + } + } +} + +template +typename pcl::PointCloud::Ptr DownsamplePointCloud(const typename pcl::PointCloud::Ptr cloud, + const double leaf_size) { + pcl::VoxelGrid voxel_grid; + voxel_grid.setInputCloud(cloud); + voxel_grid.setLeafSize(leaf_size, leaf_size, leaf_size); + typename pcl::PointCloud::Ptr downsampled_cloud(new pcl::PointCloud()); + voxel_grid.filter(*downsampled_cloud); + return downsampled_cloud; +} + +template +typename pcl::PointCloud::Ptr BilateralFilterOrganizedCloud( + const typename pcl::PointCloud::Ptr cloud, const double sigma_s, const double sigma_r) { + typename pcl::PointCloud::Ptr downsampled_cloud(new pcl::PointCloud()); + typename pcl::FastBilateralFilter bilateral_filter; + bilateral_filter.setInputCloud(cloud); + bilateral_filter.setSigmaS(sigma_s); + bilateral_filter.setSigmaR(sigma_r); + bilateral_filter.filter(*downsampled_cloud); + return downsampled_cloud; +} + +template +void FilterCorrespondences(const typename pcl::PointCloud& input_cloud, + const typename pcl::PointCloud& target_cloud, + pcl::Correspondences& correspondences) { + for (auto correspondence_it = correspondences.begin(); correspondence_it != correspondences.end();) { + const auto& input_point = (input_cloud)[correspondence_it->index_query]; + const auto& target_point = (target_cloud)[correspondence_it->index_match]; + const bool invalid_correspondence = !std::isfinite(input_point.x) || !std::isfinite(input_point.y) || + !std::isfinite(input_point.z) || !std::isfinite(target_point.x) || + !std::isfinite(target_point.y) || !std::isfinite(target_point.z) || + !std::isfinite(target_point.normal_x) || + !std::isfinite(target_point.normal_y) || !std::isfinite(target_point.normal_z); + if (invalid_correspondence) { + correspondence_it = correspondences.erase(correspondence_it); + continue; + } + ++correspondence_it; + } +} + +template +typename pcl::PointCloud::Ptr FilteredPointCloud( + const typename pcl::PointCloud::Ptr unfiltered_cloud) { + typename pcl::PointCloud::Ptr filtered_cloud(new pcl::PointCloud()); + pcl::copyPointCloud(*unfiltered_cloud, *filtered_cloud); + RemoveInvalidAndZeroPoints(*filtered_cloud); + return filtered_cloud; +} + +template +void EstimateNormals(const typename pcl::PointCloud::Ptr cloud, const double search_radius, + typename pcl::PointCloud& cloud_with_normals) { + typename pcl::NormalEstimation ne; + ne.setInputCloud(cloud); + typename pcl::search::KdTree::Ptr tree(new pcl::search::KdTree()); + ne.setSearchMethod(tree); + ne.setRadiusSearch(search_radius); + pcl::PointCloud cloud_normals; + ne.compute(cloud_normals); + pcl::concatenateFields(*cloud, cloud_normals, cloud_with_normals); +} + +template +void EstimateOrganizedNormals( + const typename pcl::PointCloud::Ptr cloud, + const typename pcl::IntegralImageNormalEstimation::NormalEstimationMethod method, + const bool depth_dependent_smoothing, const double max_depth_change_factor, const double normal_smoothing_size, + typename pcl::PointCloud& cloud_with_normals) { + typename pcl::IntegralImageNormalEstimation ne; + ne.setNormalEstimationMethod(method); + ne.setMaxDepthChangeFactor(max_depth_change_factor); + ne.setNormalSmoothingSize(normal_smoothing_size); + ne.setDepthDependentSmoothing(depth_dependent_smoothing); + ne.setInputCloud(cloud); + // Avoids issue where even though organized neighbor isn't used by normal estimation + // it attempts to estimate the camera intrinsics and sometimes fails + typename pcl::search::OrganizedNeighbor2::Ptr organized_neighbor( + new pcl::search::OrganizedNeighbor2()); + ne.setSearchMethod(organized_neighbor); + pcl::PointCloud cloud_normals; + ne.compute(cloud_normals); + pcl::concatenateFields(*cloud, cloud_normals, cloud_with_normals); +} + +template +void NormalSpaceSubsampling(typename pcl::PointCloud::Ptr cloud, const int bins_per_axis, + const int num_samples) { + typename pcl::NormalSpaceSampling normal_space_sampling; + normal_space_sampling.setInputCloud(cloud); + normal_space_sampling.setNormals(cloud); + normal_space_sampling.setBins(bins_per_axis, bins_per_axis, bins_per_axis); + normal_space_sampling.setSample(num_samples); + normal_space_sampling.filter(*cloud); +} + +template +typename pcl::PointCloud::Ptr FilteredPointCloudWithNormals( + const typename pcl::PointCloud::Ptr unfiltered_cloud, const double search_radius) { + typename pcl::PointCloud::Ptr filtered_cloud = FilteredPointCloud(unfiltered_cloud); + typename pcl::PointCloud::Ptr filtered_cloud_with_normals( + new pcl::PointCloud()); + EstimateNormals(filtered_cloud, search_radius, *filtered_cloud_with_normals); + RemoveInvalidAndZeroPoints(*filtered_cloud_with_normals); + return filtered_cloud_with_normals; +} + +template +Eigen::Vector3d Vector3d(const PointType& point) { + return Eigen::Vector3d(point.x, point.y, point.z); +} + +template +Eigen::Vector3d NormalVector3d(const PointType& point_with_normal) { + return Eigen::Vector3d(point_with_normal.normal[0], point_with_normal.normal[1], point_with_normal.normal[2]); +} + +template +boost::optional GetNormal(const Eigen::Vector3d& point, const pcl::PointCloud& cloud, + const pcl::search::KdTree& kdtree, const double search_radius) { + // Adapted from pcl code + PointType pcl_point; + pcl_point.x = point.x(); + pcl_point.y = point.y(); + pcl_point.z = point.z(); + + std::vector nn_indices; + std::vector distances; + if (kdtree.radiusSearch(pcl_point, search_radius, nn_indices, distances, 0) < 3) { + LogDebug("GetNormal: Failed to get enough neighboring points for query point."); + return boost::none; + } + + float normal_x; + float normal_y; + float normal_z; + float curvature; + if (!computePointNormal(cloud, nn_indices, normal_x, normal_y, normal_z, curvature)) { + LogDebug("GetNormal: Failed to compute point normal."); + return boost::none; + } + + const double vpx = cloud.sensor_origin_.coeff(0); + const double vpy = cloud.sensor_origin_.coeff(1); + const double vpz = cloud.sensor_origin_.coeff(2); + flipNormalTowardsViewpoint(pcl_point, vpx, vpy, vpz, normal_x, normal_y, normal_z); + return Eigen::Vector3d(normal_x, normal_y, normal_z); +} + +template +bool computePointNormal(const pcl::PointCloud& cloud, const std::vector& indices, float& normal_x, + float& normal_y, float& normal_z, float& curvature) { + // Adapted from pcl::common::centroid.h, should be a static or free function but is a member function instead + /** \brief Placeholder for the 3x3 covariance matrix at each surface patch. */ + static EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix_; + /** \brief 16-bytes aligned placeholder for the XYZ centroid of a surface patch. */ + static Eigen::Vector4f xyz_centroid_; + if (indices.size() < 3 || + pcl::computeMeanAndCovarianceMatrix(cloud, indices, covariance_matrix_, xyz_centroid_) == 0) { + return false; + } + + // Get the plane normal and surface curvature + pcl::solvePlaneParameters(covariance_matrix_, normal_x, normal_y, normal_z, curvature); + return true; +} +} // namespace point_cloud_common +#endif // POINT_CLOUD_COMMON_UTILITIES_H_ diff --git a/localization/point_cloud_common/package.xml b/localization/point_cloud_common/package.xml new file mode 100644 index 0000000000..abb3d3d442 --- /dev/null +++ b/localization/point_cloud_common/package.xml @@ -0,0 +1,35 @@ + + point_cloud_common + 1.0.0 + + The point cloud common package + + + Apache License, Version 2.0 + + + Astrobee Flight Software + + + Astrobee Flight Software + + catkin + ff_msgs + ff_util + localization_common + msg_conversions + nodelet + optimization_common + pcl_conversions + pcl + roscpp + ff_msgs + ff_util + localization_common + msg_conversions + nodelet + optimization_common + pcl_conversions + pcl + roscpp + diff --git a/localization/point_cloud_common/readme.md b/localization/point_cloud_common/readme.md new file mode 100644 index 0000000000..02b7ac8aae --- /dev/null +++ b/localization/point_cloud_common/readme.md @@ -0,0 +1,12 @@ +\page pointcloudcommon Point Cloud Common + +# Package Overview + +### PointToPlaneICP +Performs point to plane ICP using the PCL library. Supports nonsymmetric and symmetric costs along with optional correspondence rejection when a symmetric cost is used. Also has options for downsampling the input point clouds and performing coarse to fine ICP. + +### PointCloudWithKnownCorrespondencesAligner +Performs nonliner optimization with a robust huber cost to align two sets of corresponding 3d points (point to point cost). This is useful when correspondences between two point clouds are known and do not need to be reestimated iteratively as is done in ICP, resulting in an often much faster aligment. Supports performing an initial guess using the Umeyama algorithm. Additionally supports using a point to plane or symmetric point to plane cost, requiring surface normals in addition to 3d correspondences as needed. + +### Utilities +Provides various utilities for point cloud operations such as normal estimation, downsampling, valid point filtering, covariance estimation, and more. diff --git a/localization/point_cloud_common/src/parameter_reader.cc b/localization/point_cloud_common/src/parameter_reader.cc new file mode 100644 index 0000000000..21a86e47e2 --- /dev/null +++ b/localization/point_cloud_common/src/parameter_reader.cc @@ -0,0 +1,64 @@ +/* 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. + */ + +#include +#include +#include + +namespace point_cloud_common { +namespace mc = msg_conversions; + +void LoadPointToPlaneICPParams(config_reader::ConfigReader& config, PointToPlaneICPParams& params) { + params.use_fitness_threshold_rejection = mc::LoadBool(config, "use_fitness_threshold_rejection"); + params.fitness_threshold = mc::LoadDouble(config, "fitness_threshold"); + params.search_radius = mc::LoadDouble(config, "search_radius"); + params.max_iterations = mc::LoadInt(config, "max_iterations"); + params.symmetric_objective = mc::LoadBool(config, "symmetric_objective"); + params.enforce_same_direction_normals = mc::LoadBool(config, "enforce_same_direction_normals"); + params.correspondence_rejector_surface_normal = mc::LoadBool(config, "correspondence_rejector_surface_normal"); + params.correspondence_rejector_surface_normal_threshold = + mc::LoadDouble(config, "correspondence_rejector_surface_normal_threshold"); + params.correspondence_rejector_median_distance = mc::LoadBool(config, "correspondence_rejector_median_distance"); + params.correspondence_rejector_median_distance_factor = + mc::LoadDouble(config, "correspondence_rejector_median_distance_factor"); + params.coarse_to_fine = mc::LoadBool(config, "coarse_to_fine"); + params.num_coarse_to_fine_levels = mc::LoadInt(config, "num_coarse_to_fine_levels"); + params.coarse_to_fine_final_leaf_size = mc::LoadDouble(config, "coarse_to_fine_final_leaf_size"); + params.downsample_last_coarse_to_fine_iteration = mc::LoadBool(config, "downsample_last_coarse_to_fine_iteration"); +} + +void LoadPointCloudWithKnownCorrespondencesAlignerParams(config_reader::ConfigReader& config, + PointCloudWithKnownCorrespondencesAlignerParams& params) { + params.max_num_iterations = mc::LoadInt(config, "pcwkca_max_num_iterations"); + params.function_tolerance = mc::LoadDouble(config, "pcwkca_function_tolerance"); + params.max_num_matches = mc::LoadInt(config, "pcwkca_max_num_match_sets"); + params.normal_search_radius = mc::LoadDouble(config, "pcwkca_normal_search_radius"); + params.use_umeyama_initial_guess = mc::LoadBool(config, "pcwkca_use_umeyama_initial_guess"); + params.use_single_iteration_umeyama = mc::LoadBool(config, "pcwkca_use_single_iteration_umeyama"); + params.use_point_to_plane_cost = mc::LoadBool(config, "pcwkca_use_point_to_plane_cost"); + params.use_symmetric_point_to_plane_cost = mc::LoadBool(config, "pcwkca_use_symmetric_point_to_plane_cost"); + if (!mc::SingleBoolTrue({params.use_single_iteration_umeyama, params.use_point_to_plane_cost, + params.use_symmetric_point_to_plane_cost}) && + (params.use_single_iteration_umeyama || params.use_point_to_plane_cost || + params.use_symmetric_point_to_plane_cost)) + LogFatal( + "LoadPointCloudWithKnownCorrespondencesAlignerParams: Multiple solver methods enabled (only one of single " + "iteration umeyama, point to plane cost, and symmetric point to plane cost can be enabled)."); + params.verbose_optimization = mc::LoadBool(config, "pcwkca_verbose_optimization"); +} +} // namespace point_cloud_common diff --git a/localization/point_cloud_common/src/point_cloud_with_known_correspondences_aligner.cc b/localization/point_cloud_common/src/point_cloud_with_known_correspondences_aligner.cc new file mode 100644 index 0000000000..b8e7d34f13 --- /dev/null +++ b/localization/point_cloud_common/src/point_cloud_with_known_correspondences_aligner.cc @@ -0,0 +1,136 @@ +/* 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. + */ + +#include +#include +#include +#include +#include +#include + +namespace point_cloud_common { +namespace lc = localization_common; +namespace oc = optimization_common; + +PointCloudWithKnownCorrespondencesAligner::PointCloudWithKnownCorrespondencesAligner( + const PointCloudWithKnownCorrespondencesAlignerParams& params) + : params_(params) {} + +boost::optional PointCloudWithKnownCorrespondencesAligner::Align( + const std::vector& source_points, const std::vector& target_points, + const Eigen::Isometry3d& initial_target_T_source_estimate, + const boost::optional&> source_normals, + const boost::optional&> target_normals) const { + Eigen::Matrix target_T_source = oc::VectorFromIsometry3d(initial_target_T_source_estimate); + ceres::Problem problem; + problem.AddParameterBlock(target_T_source.data(), 6); + if (params_.use_symmetric_point_to_plane_cost) { + if (!target_normals || !source_normals) + LogFatal( + "Align: Attempting to use symmetric point to plane cost without having valid source and target normals."); + for (int i = 0; i < static_cast(source_points.size()) && i < params_.max_num_matches; ++i) { + oc::SymmetricPointToPlaneError::AddCostFunction(source_points[i], target_points[i], (*source_normals)[i], + (*target_normals)[i], target_T_source, problem); + } + } else if (params_.use_point_to_plane_cost) { + if (!target_normals) LogFatal("Align: Attempting to use point to plane cost without having valid target normals."); + for (int i = 0; i < static_cast(source_points.size()) && i < params_.max_num_matches; ++i) { + oc::PointToPlaneError::AddCostFunction(source_points[i], target_points[i], (*target_normals)[i], target_T_source, + problem); + } + } else { + for (int i = 0; i < static_cast(source_points.size()) && i < params_.max_num_matches; ++i) { + oc::PointToPointError::AddCostFunction(source_points[i], target_points[i], target_T_source, problem); + } + } + + ceres::Solver::Options options; + options.linear_solver_type = ceres::DENSE_QR; + options.max_num_iterations = params_.max_num_iterations; + options.function_tolerance = params_.function_tolerance; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + if (params_.verbose_optimization) { + std::cout << summary.FullReport() << "\n"; + int residual_size; + // TODO(rsousssan): Template on this? add somewhere else? + if (params_.use_symmetric_point_to_plane_cost) { + residual_size = 2; + } else if (params_.use_point_to_plane_cost) { + residual_size = 1; + } else { + residual_size = 3; + } + oc::CheckResiduals(residual_size, problem); + } + + const auto covariance = Covariance(target_T_source.data(), problem); + if (!covariance) { + LogWarning("Align: Failed to get covariance."); + return boost::none; + } + return lc::PoseWithCovariance(oc::Isometry3d(target_T_source), *covariance); +} + +boost::optional PointCloudWithKnownCorrespondencesAligner::ComputeRelativeTransform( + const std::vector& source_points, const std::vector& target_points, + const boost::optional&> source_normals, + const boost::optional&> target_normals, + const Eigen::Isometry3d& initial_target_T_source_estimate) const { + if (params_.use_single_iteration_umeyama) { + const Eigen::Isometry3d target_T_source_estimate = RelativeTransformUmeyama(source_points, target_points); + const auto covariance = PointToPointCovariance(source_points, target_T_source_estimate); + if (!covariance) { + LogError("ComputeRelativeTransform: Failed to get covariance."); + return boost::none; + } + return lc::PoseWithCovariance(target_T_source_estimate, *covariance); + } + + const Eigen::Isometry3d initial_target_T_source = params_.use_umeyama_initial_guess + ? RelativeTransformUmeyama(source_points, target_points) + : initial_target_T_source_estimate; + + return Align(source_points, target_points, initial_target_T_source, source_normals, target_normals); +} + +boost::optional +PointCloudWithKnownCorrespondencesAligner::ComputeRelativeTransform( + const std::vector& source_points, const std::vector& target_points, + const Eigen::Isometry3d& initial_target_T_source_estimate) const { + return ComputeRelativeTransform(source_points, target_points, boost::none, boost::none, + initial_target_T_source_estimate); +} + +boost::optional PointCloudWithKnownCorrespondencesAligner::Covariance( + const double* const target_T_source_data, ceres::Problem& problem) const { + ceres::Covariance::Options options; + ceres::Covariance covariance(options); + + std::vector > covariance_blocks; + covariance_blocks.push_back(std::make_pair(target_T_source_data, target_T_source_data)); + if (!covariance.Compute(covariance_blocks, &problem)) { + LogError("Covariance: Failed to compute covariance."); + return boost::none; + } + + lc::PoseCovariance pose_covariance; + covariance.GetCovarianceBlock(target_T_source_data, target_T_source_data, pose_covariance.data()); + return pose_covariance; +} +} // namespace point_cloud_common diff --git a/localization/point_cloud_common/src/test_utilities.cc b/localization/point_cloud_common/src/test_utilities.cc new file mode 100644 index 0000000000..d7fc407034 --- /dev/null +++ b/localization/point_cloud_common/src/test_utilities.cc @@ -0,0 +1,134 @@ +/* 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. + */ + +#include +#include + +namespace point_cloud_common { +namespace lc = localization_common; + +std::vector RandomPoints(const int num_points) { + std::vector points; + for (int i = 0; i < num_points; ++i) { + points.emplace_back(lc::RandomVector()); + } + return points; +} + +std::vector PlanePoints(const Eigen::Vector3d& point, const Eigen::Vector3d& width_vec, + const Eigen::Vector3d& height_vec, const double width, const double height, + const int num_width_points, const double num_height_points) { + std::vector plane_points; + const Eigen::Vector3d top_left_point = point - width / 2.0 * width_vec - height / 2.0 * height_vec; + const Eigen::Vector3d width_increment = width_vec * width / static_cast(num_width_points); + const Eigen::Vector3d height_increment = height_vec * height / static_cast(num_height_points); + for (int row = 0; row < num_height_points; ++row) { + const Eigen::Vector3d height_plane_point = top_left_point + row * height_increment; + for (int col = 0; col < num_width_points; ++col) { + const Eigen::Vector3d plane_point = height_plane_point + col * width_increment; + plane_points.emplace_back(plane_point); + } + } + return plane_points; +} + +std::pair, std::vector> RandomPointsWithNormals(const int num_points) { + std::vector points; + std::vector normals; + for (int i = 0; i < num_points; ++i) { + points.emplace_back(lc::RandomVector()); + normals.emplace_back(lc::RandomVector().normalized()); + } + return std::make_pair(points, normals); +} + +std::pair, std::vector> CubicPoints() { + std::vector cubic_points; + std::vector normals; + const Eigen::Vector3d origin(Eigen::Vector3d::Zero()); + const int num_width_points = 10; + const int num_height_points = 10; + const double width = 1.0; + const double height = 1.0; + const Eigen::Vector3d x_vec(1, 0, 0); + const Eigen::Vector3d y_vec(0, 1, 0); + const Eigen::Vector3d z_vec(0, 0, 1); + const auto xy_plane_points = PlanePoints(origin, x_vec, y_vec, width, height, num_width_points, num_height_points); + cubic_points.insert(cubic_points.end(), xy_plane_points.begin(), xy_plane_points.end()); + for (int i = 0; i < xy_plane_points.size(); ++i) normals.emplace_back(z_vec); + const auto yz_plane_points = PlanePoints(origin, y_vec, z_vec, width, height, num_width_points, num_height_points); + for (int i = 0; i < yz_plane_points.size(); ++i) normals.emplace_back(x_vec); + cubic_points.insert(cubic_points.end(), yz_plane_points.begin(), yz_plane_points.end()); + const auto xz_plane_points = PlanePoints(origin, x_vec, z_vec, width, height, num_width_points, num_height_points); + cubic_points.insert(cubic_points.end(), xz_plane_points.begin(), xz_plane_points.end()); + for (int i = 0; i < xz_plane_points.size(); ++i) normals.emplace_back(y_vec); + return std::make_pair(cubic_points, normals); +} + +pcl::PointNormal PCLPointNormal(const Eigen::Vector3d& point, const Eigen::Vector3d& normal) { + pcl::PointNormal pcl_point; + pcl_point.x = point.x(); + pcl_point.y = point.y(); + pcl_point.z = point.z(); + pcl_point.normal[0] = normal.x(); + pcl_point.normal[1] = normal.y(); + pcl_point.normal[2] = normal.z(); + return pcl_point; +} + +pcl::PointCloud::Ptr PointCloudWithNormals(const std::vector& points, + const std::vector& normals) { + pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); + for (int i = 0; i < points.size(); ++i) { + cloud->points.emplace_back(PCLPointNormal(points[i], normals[i])); + } + return cloud; +} + +PointToPlaneICPParams DefaultPointToPlaneICPParams() { + PointToPlaneICPParams params; + params.search_radius = 0.03; + params.use_fitness_threshold_rejection = false; + params.fitness_threshold = 1; + params.max_iterations = 10; + params.symmetric_objective = false; + params.enforce_same_direction_normals = false; + params.correspondence_rejector_surface_normal = false; + params.correspondence_rejector_surface_normal_threshold = 0.75; + params.correspondence_rejector_median_distance = false; + params.coarse_to_fine = false; + params.num_coarse_to_fine_levels = 2; + params.coarse_to_fine_final_leaf_size = 0.02; + params.downsample_last_coarse_to_fine_iteration = true; + return params; +} + +PointCloudWithKnownCorrespondencesAlignerParams DefaultPointCloudWithKnownCorrespondencesAlignerParams() { + PointCloudWithKnownCorrespondencesAlignerParams params; + params.max_num_iterations = 100; + params.function_tolerance = 1e-6; + params.max_num_matches = 1000000; + params.normal_search_radius = 0.03; + params.use_umeyama_initial_guess = false; + params.use_single_iteration_umeyama = false; + params.use_point_to_plane_cost = false; + params.use_symmetric_point_to_plane_cost = false; + params.verbose_optimization = false; + return params; +} +} // namespace point_cloud_common diff --git a/localization/point_cloud_common/src/utilities.cc b/localization/point_cloud_common/src/utilities.cc new file mode 100644 index 0000000000..4bb4c6ab8d --- /dev/null +++ b/localization/point_cloud_common/src/utilities.cc @@ -0,0 +1,165 @@ +/* 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. + */ + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace point_cloud_common { +namespace lc = localization_common; + +pcl::PointCloud::Ptr EstimateHistogramFeatures( + const pcl::PointCloud::Ptr cloud_with_normals) { + pcl::FPFHEstimation feature_estimator; + feature_estimator.setInputCloud(cloud_with_normals); + feature_estimator.setInputNormals(cloud_with_normals); + // TODO(rsoussan): Pass in kd tree from normal estimation? + pcl::search::KdTree::Ptr kd_tree(new pcl::search::KdTree); + feature_estimator.setSearchMethod(kd_tree); + // pcl: IMPORTANT: the radius used here has to be larger than the radius used to estimate the surface normals!!! + feature_estimator.setRadiusSearch(0.05); // 0.2?? + pcl::PointCloud::Ptr features(new pcl::PointCloud()); + feature_estimator.compute(*features); + return features; +} + +Eigen::Matrix4f RansacIA(const pcl::PointCloud::Ptr source_cloud, + const pcl::PointCloud::Ptr target_cloud) { + const auto source_features = EstimateHistogramFeatures(source_cloud); + const auto target_features = EstimateHistogramFeatures(target_cloud); + + pcl::SampleConsensusInitialAlignment sac_ia_aligner; + sac_ia_aligner.setInputSource(source_cloud); + sac_ia_aligner.setInputTarget(target_cloud); + sac_ia_aligner.setSourceFeatures(source_features); + sac_ia_aligner.setTargetFeatures(target_features); + sac_ia_aligner.setMaximumIterations(10); + sac_ia_aligner.setMinSampleDistance(0); + sac_ia_aligner.setNumberOfSamples(100); + sac_ia_aligner.setCorrespondenceRandomness(1); + pcl::PointCloud::Ptr result(new pcl::PointCloud); + sac_ia_aligner.align(*result); + // std::cout <<"sac has converged:"< final_transform = sac_ia_aligner.getFinalTransform(); + return sac_ia_aligner.getFinalTransformation(); +} + +Eigen::Matrix PointToPlaneJacobian(const gtsam::Point3& source_point, const gtsam::Vector3& normal, + const gtsam::Pose3& target_T_source) { + gtsam::Matrix H1; + target_T_source.transformFrom(source_point, H1); + return normal.transpose() * H1; +} + +Eigen::Matrix PointToPointJacobian(const gtsam::Point3& source_point, + const gtsam::Pose3& target_T_source) { + gtsam::Matrix H1; + target_T_source.transformFrom(source_point, H1); + return H1; +} + +Eigen::Isometry3d RelativeTransformUmeyama(const std::vector& source_points, + const std::vector& target_points) { + const int num_points = static_cast(source_points.size()); + Eigen::Matrix source_cloud_matrix(3, num_points); + Eigen::Matrix target_cloud_matrix(3, num_points); + for (int i = 0; i < num_points; ++i) { + const auto& source_point = source_points[i]; + source_cloud_matrix(0, i) = source_point.x(); + source_cloud_matrix(1, i) = source_point.y(); + source_cloud_matrix(2, i) = source_point.z(); + + const auto& target_point = target_points[i]; + target_cloud_matrix(0, i) = target_point.x(); + target_cloud_matrix(1, i) = target_point.y(); + target_cloud_matrix(2, i) = target_point.z(); + } + + const Eigen::Matrix target_T_source = Eigen::umeyama(source_cloud_matrix, target_cloud_matrix, false); + return Eigen::Isometry3d(target_T_source.matrix()); +} + +boost::optional> PointToPointCovariance(const std::vector& source_points, + const Eigen::Isometry3d& target_T_source) { + std::vector> jacobians; + const int num_correspondences = static_cast(source_points.size()); + for (int i = 0; i < num_correspondences; ++i) { + const Eigen::Matrix jacobian = PointToPointJacobian(source_points[i], lc::GtPose(target_T_source)); + if (!jacobian.allFinite()) continue; + jacobians.emplace_back(jacobian); + } + return lc::LeastSquaresCovariance(jacobians); +} + +boost::optional> PointToPlaneCovariance(const std::vector& source_points, + const std::vector& target_normals, + const Eigen::Isometry3d& target_T_source) { + std::vector> jacobians; + const int num_correspondences = static_cast(source_points.size()); + for (int i = 0; i < num_correspondences; ++i) { + const Eigen::Matrix jacobian = + PointToPlaneJacobian(source_points[i], target_normals[i], lc::GtPose(target_T_source)); + if (!jacobian.allFinite()) continue; + jacobians.emplace_back(jacobian); + } + return lc::LeastSquaresCovariance(jacobians); +} + +pcl::PointXYZI Interpolate(const pcl::PointXYZI& point_a, const pcl::PointXYZI& point_b, const double alpha) { + const double beta = 1.0 - alpha; + pcl::PointXYZI interpolated_point; + interpolated_point.x = beta * point_a.x + alpha * point_b.x; + interpolated_point.y = beta * point_a.y + alpha * point_b.y; + interpolated_point.z = beta * point_a.z + alpha * point_b.z; + interpolated_point.intensity = beta * point_a.intensity + alpha * point_b.intensity; + return interpolated_point; +} + +template <> +bool ValidPoint(const pcl::PointXYZ& point) { + return ValidPointXYZ(point); +} + +template <> +bool ValidPoint(const pcl::PointXYZI& point) { + return ValidPointXYZ(point) && ValidIntensity(point); +} + +template <> +bool ValidPoint(const pcl::PointNormal& point) { + return ValidPointXYZ(point) && ValidNormal(point); +} + +template <> +bool ValidPoint(const pcl::PointXYZINormal& point) { + return ValidPointXYZ(point) && ValidNormal(point) && ValidIntensity(point); +} +} // namespace point_cloud_common diff --git a/localization/point_cloud_common/test/test_point_cloud_common_utilities.cc b/localization/point_cloud_common/test/test_point_cloud_common_utilities.cc new file mode 100644 index 0000000000..4491a72e47 --- /dev/null +++ b/localization/point_cloud_common/test/test_point_cloud_common_utilities.cc @@ -0,0 +1,813 @@ +/* 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. + */ + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include + +namespace lc = localization_common; +namespace pc = point_cloud_common; +namespace sym = gtsam::symbol_shorthand; + +double PointToPlaneError(const gtsam::Point3& point_1, const gtsam::Point3& point_2, const gtsam::Vector3& normal_2, + const gtsam::Pose3& relative_transform) { + return (relative_transform * point_1 - point_2).dot(normal_2); +} + +TEST(UtilitiesTester, PointToPlaneJacobian) { + for (int i = 0; i < 500; ++i) { + const gtsam::Point3 point_1 = lc::RandomVector(); + const gtsam::Point3 point_2 = lc::RandomVector(); + const gtsam::Vector3 normal_2 = lc::RandomVector(); + const gtsam::Pose3 relative_transform = lc::RandomPose(); + const gtsam::Matrix H = pc::PointToPlaneJacobian(point_1, normal_2, relative_transform); + const auto numerical_H = gtsam::numericalDerivative11( + boost::function(boost::bind(&PointToPlaneError, point_1, point_2, normal_2, _1)), + relative_transform, 1e-5); + EXPECT_TRUE(numerical_H.isApprox(H.matrix(), 1e-6)); + } +} + +gtsam::Vector3 PointToPointError(const gtsam::Point3& point_1, const gtsam::Point3& point_2, + const gtsam::Pose3& relative_transform) { + return relative_transform * point_1 - point_2; +} + +TEST(UtilitiesTester, PointToPointJacobian) { + for (int i = 0; i < 500; ++i) { + const gtsam::Point3 point_1 = lc::RandomVector(); + const gtsam::Point3 point_2 = lc::RandomVector(); + const gtsam::Pose3 relative_transform = lc::RandomPose(); + const gtsam::Matrix H = pc::PointToPointJacobian(point_1, relative_transform); + const auto numerical_H = gtsam::numericalDerivative11( + boost::function(boost::bind(&PointToPointError, point_1, point_2, _1)), + relative_transform, 1e-5); + EXPECT_TRUE(numerical_H.isApprox(H.matrix(), 1e-6)); + } +} + +TEST(UtilitiesTester, ValidPoint_XYZ) { + // Valid + { + pcl::PointXYZ p(1, 2, 3); + EXPECT_TRUE(pc::ValidPoint(p)); + } + // Invalid nan + { + pcl::PointXYZ p(std::numeric_limits::quiet_NaN(), 2, 3); + EXPECT_FALSE(pc::ValidPoint(p)); + } + // Invalid inf + { + pcl::PointXYZ p(std::numeric_limits::infinity(), 2, 3); + EXPECT_FALSE(pc::ValidPoint(p)); + } +} + +TEST(UtilitiesTester, ValidPoint_Normal) { + // Valid + { + pcl::PointNormal p; + p.x = 1; + p.y = 2; + p.z = 3; + p.normal[0] = 1; + p.normal[1] = 2; + p.normal[2] = 3; + EXPECT_TRUE(pc::ValidPoint(p)); + } + // Invalid nan + { + // point + pcl::PointNormal p; + p.x = std::numeric_limits::quiet_NaN(); + p.y = 2; + p.z = 3; + p.normal[0] = 1; + p.normal[1] = 2; + p.normal[2] = 3; + EXPECT_FALSE(pc::ValidPoint(p)); + + // normal + p.x = 1; + p.y = 2; + p.z = 3; + p.normal[0] = std::numeric_limits::quiet_NaN(); + p.normal[1] = 2; + p.normal[2] = 3; + EXPECT_FALSE(pc::ValidPoint(p)); + } + // Invalid inf + { + pcl::PointNormal p; + // point + p.x = std::numeric_limits::infinity(); + p.y = 2; + p.z = 3; + p.normal[0] = 1; + p.normal[1] = 2; + p.normal[2] = 3; + EXPECT_FALSE(pc::ValidPoint(p)); + + // normal + p.x = 1; + p.y = 2; + p.z = 3; + p.normal[0] = std::numeric_limits::infinity(); + p.normal[1] = 2; + p.normal[2] = 3; + EXPECT_FALSE(pc::ValidPoint(p)); + } + // Invalid Zero Normal + { + pcl::PointNormal p; + p.x = 1; + p.y = 2; + p.z = 3; + p.normal[0] = 0; + p.normal[1] = 0; + p.normal[2] = 0; + EXPECT_FALSE(pc::ValidPoint(p)); + } +} + +TEST(UtilitiesTester, ValidPoint_XYZI) { + // Valid + { + pcl::PointXYZI p; + p.x = 1; + p.y = 2; + p.z = 3; + p.intensity = 4; + EXPECT_TRUE(pc::ValidPoint(p)); + } + // Invalid nan + { + pcl::PointXYZI p; + p.x = std::numeric_limits::quiet_NaN(); + p.y = 2; + p.z = 3; + p.intensity = 4; + EXPECT_FALSE(pc::ValidPoint(p)); + p.x = 1; + p.y = 2; + p.z = 3; + p.intensity = std::numeric_limits::quiet_NaN(); + EXPECT_FALSE(pc::ValidPoint(p)); + } + // Invalid inf + { + pcl::PointXYZI p; + p.x = std::numeric_limits::infinity(); + p.y = 2; + p.z = 3; + p.intensity = 1; + EXPECT_FALSE(pc::ValidPoint(p)); + + p.x = 1; + p.y = 2; + p.z = 3; + p.intensity = std::numeric_limits::infinity(); + EXPECT_FALSE(pc::ValidPoint(p)); + } +} + +TEST(UtilitiesTester, ValidPoint_XYZINormal) { + // Valid + { + pcl::PointXYZINormal p; + p.x = 1; + p.y = 2; + p.z = 3; + p.intensity = 4; + p.normal[0] = 1; + p.normal[1] = 2; + p.normal[2] = 3; + EXPECT_TRUE(pc::ValidPoint(p)); + } + // Invalid nan + { + // point + pcl::PointXYZINormal p; + p.x = std::numeric_limits::quiet_NaN(); + p.y = 2; + p.z = 3; + p.intensity = 4; + p.normal[0] = 1; + p.normal[1] = 2; + p.normal[2] = 3; + EXPECT_FALSE(pc::ValidPoint(p)); + + // intensity + p.x = 1; + p.y = 2; + p.z = 3; + p.intensity = std::numeric_limits::quiet_NaN(); + p.normal[0] = 1; + p.normal[1] = 2; + p.normal[2] = 3; + EXPECT_FALSE(pc::ValidPoint(p)); + + // normal + p.x = 1; + p.y = 2; + p.z = 3; + p.intensity = 4; + p.normal[0] = std::numeric_limits::quiet_NaN(); + p.normal[1] = 2; + p.normal[2] = 3; + EXPECT_FALSE(pc::ValidPoint(p)); + } + // Invalid inf + { + pcl::PointXYZINormal p; + // point + p.x = std::numeric_limits::infinity(); + p.y = 2; + p.z = 3; + p.intensity = 1; + p.normal[0] = 1; + p.normal[1] = 2; + p.normal[2] = 3; + EXPECT_FALSE(pc::ValidPoint(p)); + + // intensity + p.x = 1; + p.y = 2; + p.z = 3; + p.intensity = std::numeric_limits::infinity(); + p.normal[0] = 1; + p.normal[1] = 2; + p.normal[2] = 3; + EXPECT_FALSE(pc::ValidPoint(p)); + + // normal + p.x = 1; + p.y = 2; + p.z = 3; + p.intensity = 1; + p.normal[0] = std::numeric_limits::infinity(); + p.normal[1] = 2; + p.normal[2] = 3; + EXPECT_FALSE(pc::ValidPoint(p)); + } + // Invalid Zero Normal + { + pcl::PointXYZINormal p; + p.x = 1; + p.y = 2; + p.z = 3; + p.intensity = 1; + p.normal[0] = 0; + p.normal[1] = 0; + p.normal[2] = 0; + EXPECT_FALSE(pc::ValidPoint(p)); + } +} + +TEST(UtilitiesTester, ValidPointXYZ) { + // Valid + { + pcl::PointXYZ p(1, 2, 3); + EXPECT_TRUE(pc::ValidPointXYZ(p)); + } + // Invalid nan + { + pcl::PointXYZ p(std::numeric_limits::quiet_NaN(), 2, 3); + EXPECT_FALSE(pc::ValidPointXYZ(p)); + } + // Invalid inf + { + pcl::PointXYZ p(std::numeric_limits::infinity(), 2, 3); + EXPECT_FALSE(pc::ValidPointXYZ(p)); + } +} + +TEST(UtilitiesTester, ValidNormal) { + // Valid + { + pcl::PointNormal p; + p.x = 1; + p.y = 2; + p.z = 3; + p.normal[0] = 1; + p.normal[1] = 2; + p.normal[2] = 3; + EXPECT_TRUE(pc::ValidNormal(p)); + } + // Invalid nan + { + pcl::PointNormal p; + p.x = std::numeric_limits::quiet_NaN(); + p.y = 2; + p.z = 3; + p.normal[0] = 1; + p.normal[1] = 2; + p.normal[2] = 3; + EXPECT_TRUE(pc::ValidNormal(p)); + + // normal + p.x = 1; + p.y = 2; + p.z = 3; + p.normal[0] = std::numeric_limits::quiet_NaN(); + p.normal[1] = 2; + p.normal[2] = 3; + EXPECT_FALSE(pc::ValidNormal(p)); + } + // Invalid inf + { + pcl::PointNormal p; + // point + p.x = std::numeric_limits::infinity(); + p.y = 2; + p.z = 3; + p.normal[0] = 1; + p.normal[1] = 2; + p.normal[2] = 3; + EXPECT_TRUE(pc::ValidNormal(p)); + + // normal + p.x = 1; + p.y = 2; + p.z = 3; + p.normal[0] = std::numeric_limits::infinity(); + p.normal[1] = 2; + p.normal[2] = 3; + EXPECT_FALSE(pc::ValidNormal(p)); + } + // Invalid Zero Normal + { + pcl::PointNormal p; + p.x = 1; + p.y = 2; + p.z = 3; + p.normal[0] = 0; + p.normal[1] = 0; + p.normal[2] = 0; + EXPECT_FALSE(pc::ValidNormal(p)); + } +} + +TEST(UtilitiesTester, ValidIntensity) { + // Valid + { + pcl::PointXYZI p; + p.x = 1; + p.y = 2; + p.z = 3; + p.intensity = 4; + EXPECT_TRUE(pc::ValidIntensity(p)); + } + // Invalid nan + { + pcl::PointXYZI p; + p.x = std::numeric_limits::quiet_NaN(); + p.y = 2; + p.z = 3; + p.intensity = 4; + EXPECT_TRUE(pc::ValidIntensity(p)); + p.x = 1; + p.y = 2; + p.z = 3; + p.intensity = std::numeric_limits::quiet_NaN(); + EXPECT_FALSE(pc::ValidIntensity(p)); + } + // Invalid inf + { + pcl::PointXYZI p; + p.x = std::numeric_limits::infinity(); + p.y = 2; + p.z = 3; + p.intensity = 1; + EXPECT_TRUE(pc::ValidIntensity(p)); + + p.x = 1; + p.y = 2; + p.z = 3; + p.intensity = std::numeric_limits::infinity(); + EXPECT_FALSE(pc::ValidIntensity(p)); + } +} + +TEST(UtilitiesTester, ApproxZero) { + EXPECT_FALSE(pc::ApproxZero(1.0)); + EXPECT_TRUE(pc::ApproxZero(0.0)); + EXPECT_TRUE(pc::ApproxZero(1e-9)); + EXPECT_TRUE(pc::ApproxZero(1, 2)); +} + +TEST(UtilitiesTester, Pcl2EigenVec3) { + pcl::PointXYZ p(1, 2, 3); + const auto eigen_p = pc::Vector3d(p); + EXPECT_NEAR(eigen_p.x(), p.x, 1e-6); + EXPECT_NEAR(eigen_p.y(), p.y, 1e-6); + EXPECT_NEAR(eigen_p.z(), p.z, 1e-6); +} + +TEST(UtilitiesTester, Pcl2EigenNormal) { + pcl::PointNormal p; + p.normal[0] = 1; + p.normal[1] = 2; + p.normal[2] = 3; + const auto eigen_normal = pc::NormalVector3d(p); + EXPECT_NEAR(eigen_normal.x(), p.normal[0], 1e-6); + EXPECT_NEAR(eigen_normal.y(), p.normal[1], 1e-6); + EXPECT_NEAR(eigen_normal.z(), p.normal[2], 1e-6); +} + +TEST(UtilitiesTester, RemoveInvalidAndZeroPoints) { + pcl::PointXYZ p_valid(0, 1, 2); + pcl::PointXYZ p_nan(std::numeric_limits::quiet_NaN(), 2, 3); + pcl::PointXYZ p_inf(std::numeric_limits::infinity(), 2, 3); + pcl::PointXYZ p_zero(0, 0, 0); + pcl::PointCloud cloud; + cloud.points.emplace_back(p_valid); + cloud.points.emplace_back(p_nan); + cloud.points.emplace_back(p_inf); + cloud.points.emplace_back(p_zero); + ASSERT_EQ(cloud.points.size(), 4); + pc::RemoveInvalidAndZeroPoints(cloud); + ASSERT_EQ(cloud.points.size(), 1); + EXPECT_NEAR(cloud.points[0].x, p_valid.x, 1e-6); + EXPECT_NEAR(cloud.points[0].y, p_valid.y, 1e-6); + EXPECT_NEAR(cloud.points[0].z, p_valid.z, 1e-6); +} + +TEST(UtilitiesTester, FilteredPointCloud) { + pcl::PointXYZ p_valid(0, 1, 2); + pcl::PointXYZ p_nan(std::numeric_limits::quiet_NaN(), 2, 3); + pcl::PointXYZ p_inf(std::numeric_limits::infinity(), 2, 3); + pcl::PointXYZ p_zero(0, 0, 0); + pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); + cloud->points.emplace_back(p_valid); + cloud->points.emplace_back(p_nan); + cloud->points.emplace_back(p_inf); + cloud->points.emplace_back(p_zero); + ASSERT_EQ(cloud->points.size(), 4); + const auto filtered_cloud = pc::FilteredPointCloud(cloud); + ASSERT_EQ(filtered_cloud->points.size(), 1); + EXPECT_NEAR(filtered_cloud->points[0].x, p_valid.x, 1e-6); + EXPECT_NEAR(filtered_cloud->points[0].y, p_valid.y, 1e-6); + EXPECT_NEAR(filtered_cloud->points[0].z, p_valid.z, 1e-6); +} + +TEST(UtilitiesTester, FilteredPointCloudWithNormalsNoValidNormals) { + pcl::PointXYZ p_valid(0, 1, 2); + pcl::PointXYZ p_nan(std::numeric_limits::quiet_NaN(), 2, 3); + pcl::PointXYZ p_inf(std::numeric_limits::infinity(), 2, 3); + pcl::PointXYZ p_zero(0, 0, 0); + pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); + cloud->points.emplace_back(p_valid); + cloud->points.emplace_back(p_nan); + cloud->points.emplace_back(p_inf); + cloud->points.emplace_back(p_zero); + ASSERT_EQ(cloud->points.size(), 4); + constexpr double search_radius = 1.0; + const auto filtered_cloud = pc::FilteredPointCloudWithNormals(cloud, search_radius); + ASSERT_EQ(filtered_cloud->points.size(), 0); +} + +TEST(UtilitiesTester, FilteredPointCloudWithNormalsZAxisNormals) { + pcl::PointXYZ p_xy_plane_1(0, 1, 0); + pcl::PointXYZ p_xy_plane_2(0, 1.1, 0); + pcl::PointXYZ p_xy_plane_3(0.1, 1, 0); + pcl::PointXYZ p_no_neighbors(100, 200, 300); + pcl::PointXYZ p_nan(std::numeric_limits::quiet_NaN(), 2, 3); + pcl::PointXYZ p_inf(std::numeric_limits::infinity(), 2, 3); + pcl::PointXYZ p_zero(0, 0, 0); + pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); + cloud->points.emplace_back(p_xy_plane_1); + cloud->points.emplace_back(p_xy_plane_2); + cloud->points.emplace_back(p_xy_plane_3); + cloud->points.emplace_back(p_no_neighbors); + cloud->points.emplace_back(p_nan); + cloud->points.emplace_back(p_inf); + cloud->points.emplace_back(p_zero); + ASSERT_EQ(cloud->points.size(), 7); + constexpr double search_radius = 1.0; + const auto filtered_cloud = pc::FilteredPointCloudWithNormals(cloud, search_radius); + // Successful normals for each XY planar point + ASSERT_EQ(filtered_cloud->points.size(), 3); + // p1 + { + const auto& filtered_point_with_normal = filtered_cloud->points[0]; + EXPECT_NEAR(filtered_point_with_normal.x, p_xy_plane_1.x, 1e-6); + EXPECT_NEAR(filtered_point_with_normal.y, p_xy_plane_1.y, 1e-6); + EXPECT_NEAR(filtered_point_with_normal.z, p_xy_plane_1.z, 1e-6); + EXPECT_NEAR(filtered_point_with_normal.normal[0], 0, 1e-6); + EXPECT_NEAR(filtered_point_with_normal.normal[1], 0, 1e-6); + EXPECT_NEAR(filtered_point_with_normal.normal[2], 1, 1e-6); + } + // p2 + { + const auto& filtered_point_with_normal = filtered_cloud->points[1]; + EXPECT_NEAR(filtered_point_with_normal.x, p_xy_plane_2.x, 1e-6); + EXPECT_NEAR(filtered_point_with_normal.y, p_xy_plane_2.y, 1e-6); + EXPECT_NEAR(filtered_point_with_normal.z, p_xy_plane_2.z, 1e-6); + EXPECT_NEAR(filtered_point_with_normal.normal[0], 0, 1e-6); + EXPECT_NEAR(filtered_point_with_normal.normal[1], 0, 1e-6); + EXPECT_NEAR(filtered_point_with_normal.normal[2], 1, 1e-6); + } + // p3 + { + const auto& filtered_point_with_normal = filtered_cloud->points[2]; + EXPECT_NEAR(filtered_point_with_normal.x, p_xy_plane_3.x, 1e-6); + EXPECT_NEAR(filtered_point_with_normal.y, p_xy_plane_3.y, 1e-6); + EXPECT_NEAR(filtered_point_with_normal.z, p_xy_plane_3.z, 1e-6); + EXPECT_NEAR(filtered_point_with_normal.normal[0], 0, 1e-6); + EXPECT_NEAR(filtered_point_with_normal.normal[1], 0, 1e-6); + EXPECT_NEAR(filtered_point_with_normal.normal[2], 1, 1e-6); + } +} + +TEST(UtilitiesTester, EstimateNormals) { + pcl::PointXYZ p_xy_plane_1(0, 1, 0); + pcl::PointXYZ p_xy_plane_2(0, 1.1, 0); + pcl::PointXYZ p_xy_plane_3(0.1, 1, 0); + pcl::PointXYZ p_no_neighbors(100, 200, 300); + pcl::PointXYZ p_nan(std::numeric_limits::quiet_NaN(), 2, 3); + pcl::PointXYZ p_inf(std::numeric_limits::infinity(), 2, 3); + pcl::PointXYZ p_zero(0, 0, 0); + pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); + cloud->points.emplace_back(p_xy_plane_1); + cloud->points.emplace_back(p_xy_plane_2); + cloud->points.emplace_back(p_xy_plane_3); + cloud->points.emplace_back(p_no_neighbors); + cloud->points.emplace_back(p_nan); + cloud->points.emplace_back(p_inf); + cloud->points.emplace_back(p_zero); + ASSERT_EQ(cloud->points.size(), 7); + constexpr double search_radius = 1.0; + pcl::PointCloud cloud_with_normals; + pc::EstimateNormals(cloud, search_radius, cloud_with_normals); + // EstimateNormals doesn't remove points with invalid points or normals, expect same size output cloud + ASSERT_EQ(cloud_with_normals.points.size(), 7); + // p1 + { + const auto& filtered_point_with_normal = cloud_with_normals.points[0]; + EXPECT_NEAR(filtered_point_with_normal.x, p_xy_plane_1.x, 1e-6); + EXPECT_NEAR(filtered_point_with_normal.y, p_xy_plane_1.y, 1e-6); + EXPECT_NEAR(filtered_point_with_normal.z, p_xy_plane_1.z, 1e-6); + EXPECT_NEAR(filtered_point_with_normal.normal[0], 0, 1e-6); + EXPECT_NEAR(filtered_point_with_normal.normal[1], 0, 1e-6); + EXPECT_NEAR(filtered_point_with_normal.normal[2], 1, 1e-6); + } + // p2 + { + const auto& filtered_point_with_normal = cloud_with_normals.points[1]; + EXPECT_NEAR(filtered_point_with_normal.x, p_xy_plane_2.x, 1e-6); + EXPECT_NEAR(filtered_point_with_normal.y, p_xy_plane_2.y, 1e-6); + EXPECT_NEAR(filtered_point_with_normal.z, p_xy_plane_2.z, 1e-6); + EXPECT_NEAR(filtered_point_with_normal.normal[0], 0, 1e-6); + EXPECT_NEAR(filtered_point_with_normal.normal[1], 0, 1e-6); + EXPECT_NEAR(filtered_point_with_normal.normal[2], 1, 1e-6); + } + // p3 + { + const auto& filtered_point_with_normal = cloud_with_normals.points[2]; + EXPECT_NEAR(filtered_point_with_normal.x, p_xy_plane_3.x, 1e-6); + EXPECT_NEAR(filtered_point_with_normal.y, p_xy_plane_3.y, 1e-6); + EXPECT_NEAR(filtered_point_with_normal.z, p_xy_plane_3.z, 1e-6); + EXPECT_NEAR(filtered_point_with_normal.normal[0], 0, 1e-6); + EXPECT_NEAR(filtered_point_with_normal.normal[1], 0, 1e-6); + EXPECT_NEAR(filtered_point_with_normal.normal[2], 1, 1e-6); + } + // Invalid Points + // No Neighbors + { + const auto& filtered_point_with_normal = cloud_with_normals.points[3]; + EXPECT_NEAR(filtered_point_with_normal.x, p_no_neighbors.x, 1e-6); + EXPECT_NEAR(filtered_point_with_normal.y, p_no_neighbors.y, 1e-6); + EXPECT_NEAR(filtered_point_with_normal.z, p_no_neighbors.z, 1e-6); + // Invalid normal + EXPECT_TRUE(std::isnan(filtered_point_with_normal.normal[0])); + EXPECT_TRUE(std::isnan(filtered_point_with_normal.normal[1])); + ASSERT_TRUE(std::isnan(filtered_point_with_normal.normal[2])); + } + // Nan + { + const auto& filtered_point_with_normal = cloud_with_normals.points[4]; + EXPECT_TRUE(std::isnan(filtered_point_with_normal.x)); + EXPECT_NEAR(filtered_point_with_normal.y, p_nan.y, 1e-6); + EXPECT_NEAR(filtered_point_with_normal.z, p_nan.z, 1e-6); + // Invalid normal + EXPECT_TRUE(std::isnan(filtered_point_with_normal.normal[0])); + EXPECT_TRUE(std::isnan(filtered_point_with_normal.normal[1])); + EXPECT_TRUE(std::isnan(filtered_point_with_normal.normal[2])); + } + // Inf + { + const auto& filtered_point_with_normal = cloud_with_normals.points[5]; + EXPECT_FALSE(std::isfinite(filtered_point_with_normal.x)); + EXPECT_NEAR(filtered_point_with_normal.y, p_inf.y, 1e-6); + EXPECT_NEAR(filtered_point_with_normal.z, p_inf.z, 1e-6); + // Invalid normal + EXPECT_TRUE(std::isnan(filtered_point_with_normal.normal[0])); + EXPECT_TRUE(std::isnan(filtered_point_with_normal.normal[1])); + EXPECT_TRUE(std::isnan(filtered_point_with_normal.normal[2])); + } + // Zero Point + { + const auto& filtered_point_with_normal = cloud_with_normals.points[6]; + EXPECT_NEAR(filtered_point_with_normal.x, p_zero.x, 1e-6); + EXPECT_NEAR(filtered_point_with_normal.y, p_zero.y, 1e-6); + EXPECT_NEAR(filtered_point_with_normal.z, p_zero.z, 1e-6); + // Invalid normal + EXPECT_TRUE(std::isnan(filtered_point_with_normal.normal[0])); + EXPECT_TRUE(std::isnan(filtered_point_with_normal.normal[1])); + EXPECT_TRUE(std::isnan(filtered_point_with_normal.normal[2])); + } +} + +TEST(UtilitiesTester, GetNormal) { + pcl::PointXYZ p_xy_plane_1(0, 1, 0); + pcl::PointXYZ p_xy_plane_2(0, 1.1, 0); + pcl::PointXYZ p_xy_plane_3(0.1, 1, 0); + pcl::PointXYZ p_no_neighbors(100, 200, 300); + pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); + cloud->points.emplace_back(p_xy_plane_1); + cloud->points.emplace_back(p_xy_plane_2); + cloud->points.emplace_back(p_xy_plane_3); + cloud->points.emplace_back(p_no_neighbors); + ASSERT_EQ(cloud->points.size(), 4); + constexpr double search_radius = 1.0; + pcl::search::KdTree kdtree; + kdtree.setInputCloud(cloud); + + // p1 + { + const auto normal = pc::GetNormal(pc::Vector3d(p_xy_plane_1), *cloud, kdtree, search_radius); + ASSERT_TRUE(normal != boost::none); + EXPECT_NEAR(normal->x(), 0, 1e-6); + EXPECT_NEAR(normal->y(), 0, 1e-6); + EXPECT_NEAR(normal->z(), 1, 1e-6); + } + // p2 + { + const auto normal = pc::GetNormal(pc::Vector3d(p_xy_plane_2), *cloud, kdtree, search_radius); + ASSERT_TRUE(normal != boost::none); + EXPECT_NEAR(normal->x(), 0, 1e-6); + EXPECT_NEAR(normal->y(), 0, 1e-6); + EXPECT_NEAR(normal->z(), 1, 1e-6); + } + // p3 + { + const auto normal = pc::GetNormal(pc::Vector3d(p_xy_plane_3), *cloud, kdtree, search_radius); + ASSERT_TRUE(normal != boost::none); + EXPECT_NEAR(normal->x(), 0, 1e-6); + EXPECT_NEAR(normal->y(), 0, 1e-6); + EXPECT_NEAR(normal->z(), 1, 1e-6); + } + // No neighbors + { + const auto normal = pc::GetNormal(pc::Vector3d(p_no_neighbors), *cloud, kdtree, search_radius); + ASSERT_EQ(normal, boost::none); + } +} + +TEST(UtilitiesTester, FilterCorrespondences) { + pcl::PointXYZINormal p_valid; + p_valid.x = 1; + p_valid.y = 2; + p_valid.z = 3; + p_valid.intensity = 4; + p_valid.normal[0] = 1; + p_valid.normal[1] = 2; + p_valid.normal[2] = 3; + + pcl::PointXYZINormal p_nan; + p_nan.x = std::numeric_limits::quiet_NaN(); + p_nan.y = 2; + p_nan.z = 3; + p_nan.intensity = 4; + p_nan.normal[0] = 1; + p_nan.normal[1] = 2; + p_nan.normal[2] = 3; + + pcl::PointXYZINormal p_inf; + p_inf.x = std::numeric_limits::infinity(); + p_inf.y = 2; + p_inf.z = 3; + p_inf.intensity = 1; + p_inf.normal[0] = 1; + p_inf.normal[1] = 2; + p_inf.normal[2] = 3; + pcl::PointCloud cloud; + cloud.points.emplace_back(p_valid); + cloud.points.emplace_back(p_nan); + cloud.points.emplace_back(p_inf); + ASSERT_EQ(cloud.points.size(), 3); + pcl::Correspondences correspondences; + // Only 0,0 is valid + correspondences.emplace_back(pcl::Correspondence(0, 0, 1)); + correspondences.emplace_back(pcl::Correspondence(1, 0, 1)); + correspondences.emplace_back(pcl::Correspondence(0, 1, 1)); + correspondences.emplace_back(pcl::Correspondence(2, 0, 1)); + correspondences.emplace_back(pcl::Correspondence(0, 2, 1)); + pc::FilterCorrespondences(cloud, cloud, correspondences); + ASSERT_EQ(correspondences.size(), 1); + EXPECT_EQ(correspondences[0].index_query, 0); + EXPECT_EQ(correspondences[0].index_match, 0); +} + +TEST(UtilitiesTester, DownsamplePointCloud) { + pcl::PointXYZ p1(0.3, 0, 0); + pcl::PointXYZ p2(0, 0.3, 0); + pcl::PointXYZ p3(0, 0, 0.3); + pcl::PointXYZ p_no_neighbors(100, 200, 300); + pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); + cloud->points.emplace_back(p1); + cloud->points.emplace_back(p2); + cloud->points.emplace_back(p3); + cloud->points.emplace_back(p_no_neighbors); + ASSERT_EQ(cloud->points.size(), 4); + constexpr double leaf_size = 1.0; + const auto downsampled_cloud = pc::DownsamplePointCloud(cloud, leaf_size); + ASSERT_EQ(downsampled_cloud->points.size(), 2); + // Average of first three points is 0.1, 0.1, 0.1 + { + const auto& p = downsampled_cloud->points[0]; + EXPECT_NEAR(p.x, 0.1, 1e-6); + EXPECT_NEAR(p.y, 0.1, 1e-6); + EXPECT_NEAR(p.z, 0.1, 1e-6); + } + // Point with no neighbors should be preserved + { + const auto& p = downsampled_cloud->points[1]; + EXPECT_NEAR(p.x, p_no_neighbors.x, 1e-6); + EXPECT_NEAR(p.y, p_no_neighbors.y, 1e-6); + EXPECT_NEAR(p.z, p_no_neighbors.z, 1e-6); + } +} + +TEST(UtilitiesTester, Interpolate) { + pcl::PointXYZI p1; + p1.x = 0; + p1.y = 0; + p1.z = 0; + p1.intensity = 0; + + pcl::PointXYZI p2; + p2.x = 10; + p2.y = 100; + p2.z = 1000; + p2.intensity = 10000; + + { + const auto interpolated_p = pc::Interpolate(p1, p2, 0.5); + EXPECT_NEAR(interpolated_p.x, 5, 1e-6); + EXPECT_NEAR(interpolated_p.y, 50, 1e-6); + EXPECT_NEAR(interpolated_p.z, 500, 1e-6); + EXPECT_NEAR(interpolated_p.intensity, 5000, 1e-6); + } + + { + const auto interpolated_p = pc::Interpolate(p1, p2, 0.3); + EXPECT_NEAR(interpolated_p.x, 3, 1e-6); + EXPECT_NEAR(interpolated_p.y, 30, 1e-6); + EXPECT_NEAR(interpolated_p.z, 300, 1e-6); + EXPECT_NEAR(interpolated_p.intensity, 3000, 1e-6); + } +} + +TEST(UmeyamaTester, PerfectEstimate) { + for (int i = 0; i < 50; ++i) { + const auto a_T_points = pc::RandomPoints(20); + const auto b_T_a = lc::RandomIsometry3d(); + const auto b_T_points = lc::Transform(a_T_points, b_T_a); + const auto estimated_b_T_a = pc::RelativeTransformUmeyama(a_T_points, b_T_points); + EXPECT_PRED2(lc::MatrixEquality<6>, estimated_b_T_a.matrix(), b_T_a.matrix()); + } +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/point_cloud_common/test/test_point_cloud_common_utilities.test b/localization/point_cloud_common/test/test_point_cloud_common_utilities.test new file mode 100644 index 0000000000..2abc3f57b5 --- /dev/null +++ b/localization/point_cloud_common/test/test_point_cloud_common_utilities.test @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/localization/point_cloud_common/test/test_point_cloud_with_known_correspondences_aligner.cc b/localization/point_cloud_common/test/test_point_cloud_with_known_correspondences_aligner.cc new file mode 100644 index 0000000000..8e08a6246a --- /dev/null +++ b/localization/point_cloud_common/test/test_point_cloud_with_known_correspondences_aligner.cc @@ -0,0 +1,232 @@ +/* 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. + */ + +#include +#include +#include +#include +#include +#include + +#include + +namespace lc = localization_common; +namespace pc = point_cloud_common; + +TEST(PointCloudWithKnownCorrespondencesAlignerTester, PointToPointNoisyInitialEstimateCubicPoints) { + const auto params = pc::DefaultPointCloudWithKnownCorrespondencesAlignerParams(); + constexpr double translation_stddev = 1.0; + constexpr double rotation_stddev = 1.0; + pc::PointCloudWithKnownCorrespondencesAligner aligner(params); + for (int i = 0; i < 50; ++i) { + const auto source_T_points_and_normals = pc::CubicPoints(); + const auto target_T_source = lc::RandomIsometry3d(); + const auto target_T_points_and_normals = lc::TransformPointsWithNormals( + source_T_points_and_normals.first, source_T_points_and_normals.second, target_T_source); + const auto noisy_target_T_source = lc::AddNoiseToIsometry3d(target_T_source, translation_stddev, rotation_stddev); + const auto estimated_target_T_source = aligner.ComputeRelativeTransform( + source_T_points_and_normals.first, target_T_points_and_normals.first, noisy_target_T_source); + ASSERT_TRUE(estimated_target_T_source != boost::none); + EXPECT_PRED2(lc::MatrixEquality<4>, estimated_target_T_source->pose.matrix(), target_T_source.matrix()); + } +} + +TEST(PointCloudWithKnownCorrespondencesAlignerTester, PointToPointNoisyInitialEstimateRandomPoints) { + const auto params = pc::DefaultPointCloudWithKnownCorrespondencesAlignerParams(); + constexpr double translation_stddev = 1.0; + constexpr double rotation_stddev = 1.0; + constexpr int num_points = 50; + pc::PointCloudWithKnownCorrespondencesAligner aligner(params); + for (int i = 0; i < 50; ++i) { + const auto source_T_points_and_normals = pc::RandomPointsWithNormals(num_points); + const auto target_T_source = lc::RandomIsometry3d(); + const auto target_T_points_and_normals = lc::TransformPointsWithNormals( + source_T_points_and_normals.first, source_T_points_and_normals.second, target_T_source); + const auto noisy_target_T_source = lc::AddNoiseToIsometry3d(target_T_source, translation_stddev, rotation_stddev); + const auto estimated_target_T_source = aligner.ComputeRelativeTransform( + source_T_points_and_normals.first, target_T_points_and_normals.first, noisy_target_T_source); + ASSERT_TRUE(estimated_target_T_source != boost::none); + EXPECT_PRED2(lc::MatrixEquality<4>, estimated_target_T_source->pose.matrix(), target_T_source.matrix()); + } +} + +TEST(PointCloudWithKnownCorrespondencesAlignerTester, PointToPlaneNoisyInitialEstimateCubicPoints) { + auto params = pc::DefaultPointCloudWithKnownCorrespondencesAlignerParams(); + params.use_point_to_plane_cost = true; + constexpr double translation_stddev = 1.0; + constexpr double rotation_stddev = 1.0; + pc::PointCloudWithKnownCorrespondencesAligner aligner(params); + for (int i = 0; i < 50; ++i) { + const auto source_T_points_and_normals = pc::CubicPoints(); + const auto target_T_source = lc::RandomIsometry3d(); + const auto target_T_points_and_normals = lc::TransformPointsWithNormals( + source_T_points_and_normals.first, source_T_points_and_normals.second, target_T_source); + const auto noisy_target_T_source = lc::AddNoiseToIsometry3d(target_T_source, translation_stddev, rotation_stddev); + const auto estimated_target_T_source = + aligner.ComputeRelativeTransform(source_T_points_and_normals.first, target_T_points_and_normals.first, + boost::none, target_T_points_and_normals.second, noisy_target_T_source); + ASSERT_TRUE(estimated_target_T_source != boost::none); + EXPECT_PRED2(lc::MatrixEquality<4>, estimated_target_T_source->pose.matrix(), target_T_source.matrix()); + } +} + +TEST(PointCloudWithKnownCorrespondencesAlignerTester, PointToPlaneNoisyInitialEstimateRandomPoints) { + auto params = pc::DefaultPointCloudWithKnownCorrespondencesAlignerParams(); + params.use_point_to_plane_cost = true; + constexpr double translation_stddev = 1.0; + constexpr double rotation_stddev = 1.0; + constexpr int num_points = 50; + pc::PointCloudWithKnownCorrespondencesAligner aligner(params); + for (int i = 0; i < 50; ++i) { + const auto source_T_points_and_normals = pc::RandomPointsWithNormals(num_points); + const auto target_T_source = lc::RandomIsometry3d(); + const auto target_T_points_and_normals = lc::TransformPointsWithNormals( + source_T_points_and_normals.first, source_T_points_and_normals.second, target_T_source); + const auto noisy_target_T_source = lc::AddNoiseToIsometry3d(target_T_source, translation_stddev, rotation_stddev); + const auto estimated_target_T_source = + aligner.ComputeRelativeTransform(source_T_points_and_normals.first, target_T_points_and_normals.first, + boost::none, target_T_points_and_normals.second, noisy_target_T_source); + ASSERT_TRUE(estimated_target_T_source != boost::none); + EXPECT_PRED2(lc::MatrixEquality<4>, estimated_target_T_source->pose.matrix(), target_T_source.matrix()); + } +} + +TEST(PointCloudWithKnownCorrespondencesAlignerTester, SymmetricPointToPlaneNoisyInitialEstimateCubicPoints) { + auto params = pc::DefaultPointCloudWithKnownCorrespondencesAlignerParams(); + params.use_symmetric_point_to_plane_cost = true; + constexpr double translation_stddev = 1.0; + constexpr double rotation_stddev = 1.0; + pc::PointCloudWithKnownCorrespondencesAligner aligner(params); + for (int i = 0; i < 50; ++i) { + const auto source_T_points_and_normals = pc::CubicPoints(); + const auto target_T_source = lc::RandomIsometry3d(); + const auto target_T_points_and_normals = lc::TransformPointsWithNormals( + source_T_points_and_normals.first, source_T_points_and_normals.second, target_T_source); + const auto noisy_target_T_source = lc::AddNoiseToIsometry3d(target_T_source, translation_stddev, rotation_stddev); + const auto estimated_target_T_source = aligner.ComputeRelativeTransform( + source_T_points_and_normals.first, target_T_points_and_normals.first, source_T_points_and_normals.second, + target_T_points_and_normals.second, noisy_target_T_source); + ASSERT_TRUE(estimated_target_T_source != boost::none); + EXPECT_PRED2(lc::MatrixEquality<4>, estimated_target_T_source->pose.matrix(), target_T_source.matrix()); + } +} + +TEST(PointCloudWithKnownCorrespondencesAlignerTester, SymmetricPointToPlaneNoisyInitialEstimateRandomPoints) { + auto params = pc::DefaultPointCloudWithKnownCorrespondencesAlignerParams(); + params.use_symmetric_point_to_plane_cost = true; + constexpr double translation_stddev = 1.0; + constexpr double rotation_stddev = 1.0; + constexpr int num_points = 50; + pc::PointCloudWithKnownCorrespondencesAligner aligner(params); + for (int i = 0; i < 50; ++i) { + const auto source_T_points_and_normals = pc::RandomPointsWithNormals(num_points); + const auto target_T_source = lc::RandomIsometry3d(); + const auto target_T_points_and_normals = lc::TransformPointsWithNormals( + source_T_points_and_normals.first, source_T_points_and_normals.second, target_T_source); + const auto noisy_target_T_source = lc::AddNoiseToIsometry3d(target_T_source, translation_stddev, rotation_stddev); + const auto estimated_target_T_source = aligner.ComputeRelativeTransform( + source_T_points_and_normals.first, target_T_points_and_normals.first, source_T_points_and_normals.second, + target_T_points_and_normals.second, noisy_target_T_source); + ASSERT_TRUE(estimated_target_T_source != boost::none); + EXPECT_PRED2(lc::MatrixEquality<4>, estimated_target_T_source->pose.matrix(), target_T_source.matrix()); + } +} + +TEST(PointCloudWithKnownCorrespondencesAlignerTester, UmeyamaNoisyInitialEstimateCubicPoints) { + auto params = pc::DefaultPointCloudWithKnownCorrespondencesAlignerParams(); + params.use_single_iteration_umeyama = true; + constexpr double translation_stddev = 1.0; + constexpr double rotation_stddev = 1.0; + pc::PointCloudWithKnownCorrespondencesAligner aligner(params); + for (int i = 0; i < 50; ++i) { + const auto source_T_points_and_normals = pc::CubicPoints(); + const auto target_T_source = lc::RandomIsometry3d(); + const auto target_T_points_and_normals = lc::TransformPointsWithNormals( + source_T_points_and_normals.first, source_T_points_and_normals.second, target_T_source); + const auto noisy_target_T_source = lc::AddNoiseToIsometry3d(target_T_source, translation_stddev, rotation_stddev); + const auto estimated_target_T_source = aligner.ComputeRelativeTransform( + source_T_points_and_normals.first, target_T_points_and_normals.first, noisy_target_T_source); + ASSERT_TRUE(estimated_target_T_source != boost::none); + EXPECT_PRED2(lc::MatrixEquality<4>, estimated_target_T_source->pose.matrix(), target_T_source.matrix()); + } +} + +TEST(PointCloudWithKnownCorrespondencesAlignerTester, UmeyamaNoisyInitialEstimateRandomPoints) { + auto params = pc::DefaultPointCloudWithKnownCorrespondencesAlignerParams(); + params.use_single_iteration_umeyama = true; + constexpr double translation_stddev = 1.0; + constexpr double rotation_stddev = 1.0; + constexpr int num_points = 50; + pc::PointCloudWithKnownCorrespondencesAligner aligner(params); + for (int i = 0; i < 50; ++i) { + const auto source_T_points_and_normals = pc::RandomPointsWithNormals(num_points); + const auto target_T_source = lc::RandomIsometry3d(); + const auto target_T_points_and_normals = lc::TransformPointsWithNormals( + source_T_points_and_normals.first, source_T_points_and_normals.second, target_T_source); + const auto noisy_target_T_source = lc::AddNoiseToIsometry3d(target_T_source, translation_stddev, rotation_stddev); + const auto estimated_target_T_source = aligner.ComputeRelativeTransform( + source_T_points_and_normals.first, target_T_points_and_normals.first, noisy_target_T_source); + ASSERT_TRUE(estimated_target_T_source != boost::none); + EXPECT_PRED2(lc::MatrixEquality<4>, estimated_target_T_source->pose.matrix(), target_T_source.matrix()); + } +} + +TEST(PointCloudWithKnownCorrespondencesAlignerTester, PointToPointInitialUmeyamaNoisyInitialEstimateCubicPoints) { + auto params = pc::DefaultPointCloudWithKnownCorrespondencesAlignerParams(); + params.use_umeyama_initial_guess = true; + constexpr double translation_stddev = 1.0; + constexpr double rotation_stddev = 1.0; + pc::PointCloudWithKnownCorrespondencesAligner aligner(params); + for (int i = 0; i < 50; ++i) { + const auto source_T_points_and_normals = pc::CubicPoints(); + const auto target_T_source = lc::RandomIsometry3d(); + const auto target_T_points_and_normals = lc::TransformPointsWithNormals( + source_T_points_and_normals.first, source_T_points_and_normals.second, target_T_source); + const auto noisy_target_T_source = lc::AddNoiseToIsometry3d(target_T_source, translation_stddev, rotation_stddev); + const auto estimated_target_T_source = aligner.ComputeRelativeTransform( + source_T_points_and_normals.first, target_T_points_and_normals.first, noisy_target_T_source); + ASSERT_TRUE(estimated_target_T_source != boost::none); + EXPECT_PRED2(lc::MatrixEquality<4>, estimated_target_T_source->pose.matrix(), target_T_source.matrix()); + } +} + +TEST(PointCloudWithKnownCorrespondencesAlignerTester, PointToPointInitialUmeyamaNoisyInitialEstimateRandomPoints) { + auto params = pc::DefaultPointCloudWithKnownCorrespondencesAlignerParams(); + params.use_umeyama_initial_guess = true; + constexpr double translation_stddev = 1.0; + constexpr double rotation_stddev = 1.0; + constexpr int num_points = 50; + pc::PointCloudWithKnownCorrespondencesAligner aligner(params); + for (int i = 0; i < 50; ++i) { + const auto source_T_points_and_normals = pc::RandomPointsWithNormals(num_points); + const auto target_T_source = lc::RandomIsometry3d(); + const auto target_T_points_and_normals = lc::TransformPointsWithNormals( + source_T_points_and_normals.first, source_T_points_and_normals.second, target_T_source); + const auto noisy_target_T_source = lc::AddNoiseToIsometry3d(target_T_source, translation_stddev, rotation_stddev); + const auto estimated_target_T_source = aligner.ComputeRelativeTransform( + source_T_points_and_normals.first, target_T_points_and_normals.first, noisy_target_T_source); + ASSERT_TRUE(estimated_target_T_source != boost::none); + EXPECT_PRED2(lc::MatrixEquality<4>, estimated_target_T_source->pose.matrix(), target_T_source.matrix()); + } +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/point_cloud_common/test/test_point_cloud_with_known_correspondences_aligner.test b/localization/point_cloud_common/test/test_point_cloud_with_known_correspondences_aligner.test new file mode 100644 index 0000000000..f8cf43cfe2 --- /dev/null +++ b/localization/point_cloud_common/test/test_point_cloud_with_known_correspondences_aligner.test @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/localization/point_cloud_common/test/test_point_to_plane_icp.cc b/localization/point_cloud_common/test/test_point_to_plane_icp.cc new file mode 100644 index 0000000000..7e3a107dfd --- /dev/null +++ b/localization/point_cloud_common/test/test_point_to_plane_icp.cc @@ -0,0 +1,207 @@ +/* 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. + */ + +#include +#include +#include +#include +#include +#include + +#include + +#include + +namespace lc = localization_common; +namespace pc = point_cloud_common; + +TEST(PointToPlaneICPTester, NoisyInitialEstimateCubicPoints) { + const auto params = pc::DefaultPointToPlaneICPParams(); + constexpr double translation_stddev = 0.01; + constexpr double rotation_stddev = 0.01; + pc::PointToPlaneICP icp(params); + for (int i = 0; i < 50; ++i) { + const auto source_T_points_and_normals = pc::CubicPoints(); + const auto target_T_source = lc::RandomIsometry3d(); + const auto source_cloud_with_normals = + pc::PointCloudWithNormals(source_T_points_and_normals.first, source_T_points_and_normals.second); + pcl::PointCloud::Ptr target_cloud_with_normals(new pcl::PointCloud()); + pcl::transformPointCloudWithNormals(*source_cloud_with_normals, *target_cloud_with_normals, + Eigen::Affine3d(target_T_source.matrix())); + const auto noisy_target_T_source = lc::AddNoiseToIsometry3d(target_T_source, translation_stddev, rotation_stddev); + const auto estimated_target_T_source = + icp.ComputeRelativeTransform(source_cloud_with_normals, target_cloud_with_normals, noisy_target_T_source); + ASSERT_TRUE(estimated_target_T_source != boost::none); + EXPECT_PRED2(lc::MatrixEquality<2>, estimated_target_T_source->pose.matrix(), target_T_source.matrix()); + } +} + +TEST(PointToPlaneICPTester, NoisyInitialEstimateRandomPoints) { + const auto params = pc::DefaultPointToPlaneICPParams(); + constexpr double translation_stddev = 0.01; + constexpr double rotation_stddev = 0.01; + constexpr int num_points = 50; + pc::PointToPlaneICP icp(params); + for (int i = 0; i < 50; ++i) { + const auto source_T_points_and_normals = pc::RandomPointsWithNormals(num_points); + const auto target_T_source = lc::RandomIsometry3d(); + const auto source_cloud_with_normals = + pc::PointCloudWithNormals(source_T_points_and_normals.first, source_T_points_and_normals.second); + pcl::PointCloud::Ptr target_cloud_with_normals(new pcl::PointCloud()); + pcl::transformPointCloudWithNormals(*source_cloud_with_normals, *target_cloud_with_normals, + Eigen::Affine3d(target_T_source.matrix())); + const auto noisy_target_T_source = lc::AddNoiseToIsometry3d(target_T_source, translation_stddev, rotation_stddev); + const auto estimated_target_T_source = + icp.ComputeRelativeTransform(source_cloud_with_normals, target_cloud_with_normals, noisy_target_T_source); + ASSERT_TRUE(estimated_target_T_source != boost::none); + EXPECT_PRED2(lc::MatrixEquality<2>, estimated_target_T_source->pose.matrix(), target_T_source.matrix()); + } +} + +TEST(PointToPlaneICPTester, NoisyInitialEstimateRandomPointsCorrespondencesTest) { + const auto params = pc::DefaultPointToPlaneICPParams(); + constexpr double translation_stddev = 0.01; + constexpr double rotation_stddev = 0.01; + constexpr int num_points = 50; + pc::PointToPlaneICP icp(params); + for (int i = 0; i < 50; ++i) { + const auto source_T_points_and_normals = pc::RandomPointsWithNormals(num_points); + const auto target_T_source = lc::RandomIsometry3d(); + const auto source_cloud_with_normals = + pc::PointCloudWithNormals(source_T_points_and_normals.first, source_T_points_and_normals.second); + pcl::PointCloud::Ptr target_cloud_with_normals(new pcl::PointCloud()); + pcl::transformPointCloudWithNormals(*source_cloud_with_normals, *target_cloud_with_normals, + Eigen::Affine3d(target_T_source.matrix())); + const auto noisy_target_T_source = lc::AddNoiseToIsometry3d(target_T_source, translation_stddev, rotation_stddev); + const auto estimated_target_T_source = + icp.ComputeRelativeTransform(source_cloud_with_normals, target_cloud_with_normals, noisy_target_T_source); + ASSERT_TRUE(estimated_target_T_source != boost::none); + EXPECT_PRED2(lc::MatrixEquality<2>, estimated_target_T_source->pose.matrix(), target_T_source.matrix()); + const auto correspondences = icp.correspondences(); + ASSERT_TRUE(correspondences != boost::none); + for (int i = 0; i < correspondences->size(); ++i) { + const auto& source_point = correspondences->source_points[i]; + const auto& target_point = correspondences->target_points[i]; + const Eigen::Vector3d transformed_source_point = estimated_target_T_source->pose * source_point; + EXPECT_PRED2(lc::MatrixEquality<2>, target_point.matrix(), transformed_source_point.matrix()); + } + } +} + +TEST(PointToPlaneICPTester, NoisyInitialEstimateSymmetricCostCubicPoints) { + auto params = pc::DefaultPointToPlaneICPParams(); + params.symmetric_objective = true; + params.enforce_same_direction_normals = true; + constexpr double translation_stddev = 0.01; + constexpr double rotation_stddev = 0.01; + pc::PointToPlaneICP icp(params); + for (int i = 0; i < 50; ++i) { + const auto source_T_points_and_normals = pc::CubicPoints(); + const auto target_T_source = lc::RandomIsometry3d(); + const auto source_cloud_with_normals = + pc::PointCloudWithNormals(source_T_points_and_normals.first, source_T_points_and_normals.second); + pcl::PointCloud::Ptr target_cloud_with_normals(new pcl::PointCloud()); + pcl::transformPointCloudWithNormals(*source_cloud_with_normals, *target_cloud_with_normals, + Eigen::Affine3d(target_T_source.matrix())); + const auto noisy_target_T_source = lc::AddNoiseToIsometry3d(target_T_source, translation_stddev, rotation_stddev); + const auto estimated_target_T_source = + icp.ComputeRelativeTransform(source_cloud_with_normals, target_cloud_with_normals, noisy_target_T_source); + ASSERT_TRUE(estimated_target_T_source != boost::none); + EXPECT_PRED2(lc::MatrixEquality<2>, estimated_target_T_source->pose.matrix(), target_T_source.matrix()); + } +} + +TEST(PointToPlaneICPTester, NoisyInitialEstimateSymmetricCostRandomPoints) { + auto params = pc::DefaultPointToPlaneICPParams(); + params.symmetric_objective = true; + params.enforce_same_direction_normals = true; + constexpr double translation_stddev = 0.01; + constexpr double rotation_stddev = 0.01; + constexpr int num_points = 50; + pc::PointToPlaneICP icp(params); + for (int i = 0; i < 50; ++i) { + const auto source_T_points_and_normals = pc::RandomPointsWithNormals(num_points); + const auto target_T_source = lc::RandomIsometry3d(); + const auto source_cloud_with_normals = + pc::PointCloudWithNormals(source_T_points_and_normals.first, source_T_points_and_normals.second); + pcl::PointCloud::Ptr target_cloud_with_normals(new pcl::PointCloud()); + pcl::transformPointCloudWithNormals(*source_cloud_with_normals, *target_cloud_with_normals, + Eigen::Affine3d(target_T_source.matrix())); + const auto noisy_target_T_source = lc::AddNoiseToIsometry3d(target_T_source, translation_stddev, rotation_stddev); + const auto estimated_target_T_source = + icp.ComputeRelativeTransform(source_cloud_with_normals, target_cloud_with_normals, noisy_target_T_source); + ASSERT_TRUE(estimated_target_T_source != boost::none); + EXPECT_PRED2(lc::MatrixEquality<2>, estimated_target_T_source->pose.matrix(), target_T_source.matrix()); + } +} + +TEST(PointToPlaneICPTester, NoisyInitialEstimateCorrespondenceRejectorRandomPoints) { + auto params = pc::DefaultPointToPlaneICPParams(); + params.correspondence_rejector_surface_normal = true; + params.correspondence_rejector_surface_normal_threshold = 0.75; + constexpr double translation_stddev = 0.01; + constexpr double rotation_stddev = 0.01; + constexpr int num_points = 50; + pc::PointToPlaneICP icp(params); + for (int i = 0; i < 50; ++i) { + const auto source_T_points_and_normals = pc::RandomPointsWithNormals(num_points); + const auto target_T_source = lc::RandomIsometry3d(); + const auto source_cloud_with_normals = + pc::PointCloudWithNormals(source_T_points_and_normals.first, source_T_points_and_normals.second); + pcl::PointCloud::Ptr target_cloud_with_normals(new pcl::PointCloud()); + pcl::transformPointCloudWithNormals(*source_cloud_with_normals, *target_cloud_with_normals, + Eigen::Affine3d(target_T_source.matrix())); + const auto noisy_target_T_source = lc::AddNoiseToIsometry3d(target_T_source, translation_stddev, rotation_stddev); + const auto estimated_target_T_source = + icp.ComputeRelativeTransform(source_cloud_with_normals, target_cloud_with_normals, noisy_target_T_source); + ASSERT_TRUE(estimated_target_T_source != boost::none); + EXPECT_PRED2(lc::MatrixEquality<2>, estimated_target_T_source->pose.matrix(), target_T_source.matrix()); + } +} + +TEST(PointToPlaneICPTester, NoisyInitialEstimateCoarseToFineRandomPoints) { + auto params = pc::DefaultPointToPlaneICPParams(); + params.coarse_to_fine = true; + params.num_coarse_to_fine_levels = 3; + params.coarse_to_fine_final_leaf_size = 0.3; + params.downsample_last_coarse_to_fine_iteration = true; + constexpr double translation_stddev = 0.01; + constexpr double rotation_stddev = 0.01; + constexpr int num_points = 50; + pc::PointToPlaneICP icp(params); + for (int i = 0; i < 50; ++i) { + const auto source_T_points_and_normals = pc::RandomPointsWithNormals(num_points); + const auto target_T_source = lc::RandomIsometry3d(); + const auto source_cloud_with_normals = + pc::PointCloudWithNormals(source_T_points_and_normals.first, source_T_points_and_normals.second); + pcl::PointCloud::Ptr target_cloud_with_normals(new pcl::PointCloud()); + pcl::transformPointCloudWithNormals(*source_cloud_with_normals, *target_cloud_with_normals, + Eigen::Affine3d(target_T_source.matrix())); + const auto noisy_target_T_source = lc::AddNoiseToIsometry3d(target_T_source, translation_stddev, rotation_stddev); + const auto estimated_target_T_source = + icp.ComputeRelativeTransform(source_cloud_with_normals, target_cloud_with_normals, noisy_target_T_source); + ASSERT_TRUE(estimated_target_T_source != boost::none); + EXPECT_PRED2(lc::MatrixEquality<2>, estimated_target_T_source->pose.matrix(), target_T_source.matrix()); + } +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/point_cloud_common/test/test_point_to_plane_icp.test b/localization/point_cloud_common/test/test_point_to_plane_icp.test new file mode 100644 index 0000000000..6677c1828b --- /dev/null +++ b/localization/point_cloud_common/test/test_point_to_plane_icp.test @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/localization/readme.md b/localization/readme.md index 9a22d1d6c0..345920650f 100644 --- a/localization/readme.md +++ b/localization/readme.md @@ -4,6 +4,8 @@ This folder consists of various sensing algorithms which serve as input to local \subpage camera +\subpage depthodometry + \subpage graphlocalizer \subpage graphoptimizer @@ -28,8 +30,14 @@ This folder consists of various sensing algorithms which serve as input to local \subpage opticalflow +\subpage optimizationcommon + +\subpage pointcloudcommon + \subpage markertracking \subpage sparsemapping +\subpage visioncommon + \subpage vive_localization diff --git a/localization/sparse_mapping/build_map.md b/localization/sparse_mapping/build_map.md index 8a774e8fa3..66d92da9e3 100644 --- a/localization/sparse_mapping/build_map.md +++ b/localization/sparse_mapping/build_map.md @@ -56,7 +56,7 @@ temporarily modify the above files to reflect your camera's parameters (without checking in your changes). More details on these and other environmental variables can be found -in the \subpage astrobee configuration documentation. +in the \ref astrobee configuration documentation. ## Reduce the number of images @@ -77,7 +77,7 @@ should inspect the images in the ``eog`` viewer, and delete redundant ones from it manually, using the Delete key. The images can also be inspected and deleted with ``nvm_visualize``, a -tool included with this software. See \subpage sparsemapping for +tool included with this software. See \ref sparsemapping for details. This tool, unlike ``eog``, echoes each image name as it is displayed, which can be useful with image manipulation tasks. @@ -123,7 +123,7 @@ before doing feature detection. It was shown to create maps that are more robust to illumination changes. In practice, the map is build in pieces, and then merged. Then the -above process needs to be modified. See \subpage sparsemapping for the +above process needs to be modified. See \ref sparsemapping for the procedure. ### Map building pipeline @@ -212,7 +212,7 @@ would drift from each other. If it is desired to take out images from the map, it should happen at this stage, before the vocabulary database and pruning happens at the -next step. See \subpage sparsemapping when it comes to such +next step. See \ref sparsemapping when it comes to such operations, where the script grow_map.py is used. #### Vocabulary database @@ -304,8 +304,8 @@ The xyz locations of the control points for the granite lab, the ISS and MGTF are mentioned below. If a set of world coordinates needs to be acquired, one can use the -\subpage total_station. (Alternatively one can can try the -\subpage faro instrument but that is more technically involved.) +\ref total_station. (Alternatively one can can try the +\ref faro instrument but that is more technically involved.) Register the map with the command: @@ -366,7 +366,7 @@ new image set. ### Registration in the granite lab See the xyz coordinates of the control points used for registration in -the \subpage granite_lab_registration section. +the \ref granite_lab_registration section. ### Registration on the ISS @@ -427,7 +427,7 @@ Python command will refresh them. ### Registration in the MGTF A set of 10 registration points were measured in the MGTF with the -\subpage total_station. They are in the file: +\ref total_station. They are in the file: $ASTROBEE_SOURCE_PATH/localization/sparse_mapping/mgtf_registration.txt @@ -496,23 +496,25 @@ Then launch localization: roslaunch astrobee astrobee.launch llp:=disabled mlp:=mlp \ nodes:=framestore,dds_ros_bridge,localization_node -### Enable localization and the mapped landmark production (on MLP) - - export ROS_MASTER_URI=http://llp:11311 - rosservice call /loc/ml/enable true - -If this command returns an error saying that the service is not -available, wait a little and try again. - ### Play the bags (on MLP) cd /data/bags/directory_of_bags export ROS_MASTER_URI=http://llp:11311 - rosbag play --loop *.bag \ + rosbag play --clock --loop *.bag \ /mgt/img_sampler/nav_cam/image_record:=/hw/cam_nav \ /loc/ml/features:=/loc/ml/old_features \ /loc/ml/registration:=/loc/ml/old_registration +### Enable localization and the mapped landmark production (on MLP) + +This must happen after the bags start playing: + + export ROS_MASTER_URI=http://llp:11311 + rosservice call /loc/ml/enable true + +If this command returns an error saying that the service is not +available, wait a little and try again. + It is important to check the topics that were recorded to the bag. If the nav camera was recorded on /mgt/img_sampler/nav_cam/image_record instead of /hw/cam_nav, as it happens when recording data on the ISS, @@ -546,6 +548,8 @@ run things on the robot, but use instead a local machine. This should result on similar results as on the robot, but the speed of computations may differ. +### Preparation + Set up the environment in every terminal that is used. Ensure that you use the correct robot name below. @@ -566,19 +570,49 @@ Sym link the map to test: rm -fv $ASTROBEE_SOURCE_PATH/astrobee/resources/maps/iss.map ln -s $(pwd)/mymap.map $ASTROBEE_SOURCE_PATH/astrobee/resources/maps/iss.map -Start the localization node: +### Start localization - roslaunch astrobee astrobee.launch mlp:=local llp:=disabled \ - nodes:=framestore,localization_node robot:=$ASTROBEE_ROBOT + roslaunch astrobee astrobee.launch mlp:=local llp:=disabled \ + nodes:=framestore,localization_node robot:=$ASTROBEE_ROBOT \ + output:=screen Note how we specify the robot name at the end. -Enable localization: +### Play the bag + +As above, one must play a bag with the ``--clock`` option, while +redirecting the existing /loc topics, and ensure that the images are +published on /hw/cam_nav: + + rosbag play --clock mybag.bag \ + /mgt/img_sampler/nav_cam/image_record:=/hw/cam_nav \ + /loc/ml/features:=/loc/ml/old_features \ + /loc/ml/registration:=/loc/ml/old_registration + +### Enable localization + +Run: rosservice call /loc/ml/enable true -Then, as above, one must play a bag while redirecting the existing -/loc topics, and ensure that the images are published on /hw/cam_nav. +If this fails, try again in a little while. + +### Alternative approach + +The steps of launching localization, playing the bag, and enabling +localization can also be run from a launch file, as follows: + + roslaunch $ASTROBEE_SOURCE_PATH/astrobee/launch/offline_localization/sparse_mapping_matching_from_bag.launch \ + bagfile:=$(pwd)/mybag.bag \ + robot:=$ASTROBEE_ROBOT \ + output:=screen + +It is very important that an absolute path to the bag be used, +otherwise this command will fail. Errors about failing to start the +rosservice to enable localization can be ignored, as that service will +be started until it succeeds. + +### Examining the results The poses of the newly localized camera images can be displayed as: @@ -590,7 +624,7 @@ and compared to the old ones via: ## Evaluating the map without running the localization node -See the \subpage ekfbag page for how to run the ``sparse_map_eval`` +See the \ref ekfbag page for how to run the ``sparse_map_eval`` tool that takes as inputs a bag and a BRISK map and prints the number of detected features. diff --git a/localization/sparse_mapping/readme.md b/localization/sparse_mapping/readme.md index 10164f5c98..1939a2f2dc 100644 --- a/localization/sparse_mapping/readme.md +++ b/localization/sparse_mapping/readme.md @@ -19,7 +19,7 @@ features detected in the image and their 3D coordinates. ### Inputs -* `/hw/cam_nav`: Camera images The map file. See the \subpage +* `/hw/cam_nav`: Camera images The map file. See the \ref map_building section (towards the bottom) for its assumed location on the robot. @@ -106,9 +106,12 @@ name can change. ### Building a map -The ``build_map`` tools is used to construct a map. See \subpage +The ``build_map`` tools is used to construct a map. See \ref map_building for further details. +See \ref theia_map for how to build a map with the external +Theia package and import it into Astrobee. + ### Visualization To visualize a map, or just a list of images, use the command: @@ -154,15 +157,15 @@ disables the arrow keys, then one can navigate with the "Ins" and "Del" keys on the numpad.) This tool can be invoked to look at just images, without any map being -built. It can also delete images in this mode, with the 'Delete' and -'x' keys, if invoked as: +built. It can also delete images in this mode, with the 'x' key, if +invoked as: nvm_visualize -enable_image_deletion /*jpg ### Localize a single frame All the commands below assume that the environment was set up, -as specified in the \subpage map_building section. +as specified in the \ref map_building section. To test localization of a single frame, use the command: @@ -212,7 +215,7 @@ for speed and here we want more accuracy. ### Testing localization using a bag -See the \subpage ekfbag page for how to study how well a BRISK map +See the \ref ekfbag page for how to study how well a BRISK map with a vocabulary database does when localizing images from a bag. ### Extract sub-maps @@ -260,7 +263,7 @@ Given a set of SURF maps, they can be merged using the command: -num_image_overlaps_at_endpoints 50 It is very important to note that only maps with SURF features (see -\subpage map_building) can be merged. If a map has BRISK features, it +\ref map_building) can be merged. If a map has BRISK features, it needs to be rebuilt with SURF features, as follows: build_map -rebuild -histogram_equalization \ @@ -337,7 +340,7 @@ maps using the command: images/*jpg -output_map examine them individually, merging them as appropriate, then -performing bundle adjustment and registration as per the \subpage +performing bundle adjustment and registration as per the \ref map_building section. Only when a good enough map is obtained, a renamed copy of it should be rebuilt with BRISK features and a vocabulary database to be used on the robot. @@ -492,3 +495,9 @@ Instead of taking images out of the map randomly, one can start with a reduced map with a small list of desired images which can be set with -image_list, and then all images for which localization fails will be added back to it. + +\subpage map_building +\subpage total_station +\subpage granite_lab_registration +\subpage faro +\subpage theia_map \ No newline at end of file diff --git a/localization/sparse_mapping/theia_map.md b/localization/sparse_mapping/theia_map.md index 181b16f308..45cce2a3cf 100644 --- a/localization/sparse_mapping/theia_map.md +++ b/localization/sparse_mapping/theia_map.md @@ -2,11 +2,11 @@ Theia (https://github.com/sweeneychris/TheiaSfM) is a package for global structure-from-motion (SfM). It may be faster and require less -user effort than the method used in Astrobee which consists of -creating submaps using incremental SfM on sequences of images that -overlap with their immediate neighbors, followed by merging the -submaps. Theia uses "cascade matching" to handle non-sequential image -acquisitions. +user effort than the method used in Astrobee (see \ref map_building) +which consists of creating submaps using incremental SfM on sequences +of images that overlap with their immediate neighbors, followed by +merging the submaps. Theia uses "cascade matching" to handle +non-sequential image acquisitions. The Theia approach was tested on about 700 images acquired at different times and it did well. It is currently studied as an @@ -14,6 +14,13 @@ alternative to Astrobee's approach. # Install Theia's prerequisites +It is suggested to use ``conda`` to install Theia's dependencies. The +conda toolset does not require root access and creates a set of +consistent libraries at a custom location in your home directory, +which are largely independent of your particular Linux system. + +For the record, the instructions below were tested on Ubuntu 18.04. + Fetch and install ``conda`` from: https://docs.conda.io/projects/conda/en/latest/user-guide/install/linux.html @@ -27,84 +34,173 @@ Create and activate the environment: Run the following command to install some packages and GCC 11: - conda install -c conda-forge cmake \ - gcc_linux-64==11.1.0 gxx_linux-64==11.1.0 \ - lapack blas eigen==3.3.7 suitesparse rapidjson \ - glog gflags rocksdb OpenImageIO \ - ceres-solver=1.14.0=h0948850_10 + conda install -c conda-forge cmake \ + gcc_linux-64==11.1.0 gxx_linux-64==11.1.0 \ + lapack blas eigen==3.3.7 suitesparse rapidjson \ + glog gflags rocksdb OpenImageIO \ + ceres-solver=1.14.0=h0948850_10 The Ceres package can be quite particular about the version of Eigen -it uses, and some versions of Ceres are not built with suitesparse +it uses, and some versions of Ceres are not built with ``suitesparse``, which is a sparse solver that is needed for best performance, so some care is needed with choosing the versions of the packages. # Fetch and build Theia - git clone git@github.com:sweeneychris/TheiaSfM.git - cd TheiaSfM - git checkout d2112f1 # this version was tested + git clone git@github.com:sweeneychris/TheiaSfM.git + cd TheiaSfM + git checkout d2112f1 # this version was tested Edit the file: - applications/CMakeLists.txt + applications/CMakeLists.txt and comment out all the lines regarding OpenGL, GLEW, GLUT, and view_reconstruction. That visualizer logic is not easy to compile and is not needed. Run ``which cmake`` to ensure its version in the ``theia`` environemnt -installed earlier is used. Otherwise run again ``conda activate -theia``. Do: +installed earlier is used. Otherwise run again: + + conda activate theia + +Do: - mkdir build - cd build - cmake .. - make -j 20 + mkdir build + cd build + cmake .. + make -j 20 This will create the executables ``build_reconstruction`` and -``export_to_nvm_file`` in the ``bin`` subdirectory of ``build``. That -directory needs to be added to the PATH. +``export_to_nvm_file`` in the ``bin`` subdirectory of ``build``. -# Run the Astrobee wrapper around the Theia tools +# Hide the conda environment The conda environment set up earlier will confuse the lookup the -dependencies for the Astrobee libraries. Hence the lines ``conda`` added -to one's ``.bashrc`` should be removed, the bash shell restarted, and -one should ensure that the ``env`` command has no mentions of conda. +dependencies for the Astrobee libraries. Hence remove the block of +lines starting and ending with ``conda initialize`` which conda added +to your ~/.bashrc, then close and reopen your terminal. Ensure that +the ``env`` command shows no mention of conda. + +# Set up the environment for Theia and Astrobee + +Add the Theia tools to your path as: + + export PATH=/path/to/TheiaSfM/build/bin:$PATH + +Set up the environment for Astrobee, including the robot name. These +should be adjusted as needed: + + export ASTROBEE_BUILD_PATH=$HOME/astrobee + export ASTROBEE_SOURCE_PATH=$ASTROBEE_BUILD_PATH/src + source $ASTROBEE_BUILD_PATH/devel/setup.bash + export ASTROBEE_RESOURCE_DIR=$ASTROBEE_SOURCE_PATH/astrobee/resources + export ASTROBEE_CONFIG_DIR=$ASTROBEE_SOURCE_PATH/astrobee/config + export ASTROBEE_WORLD=iss + export ASTROBEE_ROBOT=bumble + +Add some Astrobee tools to the path as well: + + export PATH=$ASTROBEE_BUILD_PATH/devel/lib/sparse_mapping:$PATH + +Please note that if the robot name, as specified in +``ASTROBEE_ROBOT``, is incorrect, you will get poor results. + +# Prepare the data + +The data preparation is the same as used with the usual Astrobee +map-building software documented in \ref map_building. To summarize, +the steps are as follows. + +Extract the data from the bag: -Set the environment. The following lines should be adjusted as needed, -especially the robot name: + $ASTROBEE_BUILD_PATH/devel/lib/localization_node/extract_image_bag \ + bagfile.bag -use_timestamp_as_image_name \ + -image_topic /hw/cam_nav -output_directory image_dir - export ASTROBEE_SOURCE_PATH=$HOME/projects/astrobee/src - source $ASTROBEE_SOURCE_PATH/../devel/setup.bash - export ASTROBEE_RESOURCE_DIR=$ASTROBEE_SOURCE_PATH/astrobee/resources - export ASTROBEE_CONFIG_DIR=$ASTROBEE_SOURCE_PATH/astrobee/config - export ASTROBEE_WORLD=iss - export ASTROBEE_ROBOT=bumble +If the images were recorded with the image sampler, the nav_cam image +topic needs to be changed to: - python $ASTROBEE_SOURCE_PATH/localization/sparse_mapping/tools/build_theia_map.py \ - --output_map theia.map --work_dir theia_work --image_list image_list.txt + /mgt/img_sampler/nav_cam/image_record + +The ``-use_timestamp_as_image_name`` option is not strictly necessary, +but it is helpful if a lot of datasets needs to be processed +jointly. With it, the image filename is the timestamp (in +double-precision seconds since epoch), which provides for a rather +unique name, as compared to using the image index in the bag without +that option. + +Select a subset of the images: + + select_images -density_factor 1.4 image_dir/*.jpg + +This will delete a lot of similar images from that directory. This is +not a foolproof process, and sometimes too many images are deleted but +most of the time too many are left. It is suggested to open the +remaining images with ``eog`` as: + + eog image_dir/*.jpg + +and use the ``Delete`` key to remove redundant ones. The +``nvm_visualize`` tool (see \ref sparsemapping) can be used exactly as +``eog``, and it has the advantage that it echoes the current image +name in the terminal, which can be useful in some occasions. + +A good rule of thumb is for each image to have a 4/5 overlap with the +next one. Too much or too little overlap will cause Theia to fail. + +It is suggested to avoid images with pure camera rotation, or at least +to have in the mix additional images of the same environemnt without +such rotations. + +Put the selected images in a list: + + ls image_dir/*jpg > image_list.txt + +# Run the Astrobee wrapper around the Theia tools + + python $ASTROBEE_SOURCE_PATH/localization/sparse_mapping/tools/build_theia_map.py \ + --output_map theia.map --work_dir theia_work --image_list image_list.txt This will take care of preparing everything Theia needs, will run it, -and will export the resulting map to Astrobee's expected format. This -map will need to be registered and visualized as described in other -documentation. The work directory can be deleted later. +and will export the resulting map to Astrobee's expected format. + +It is suggested to first run this tool on a small subset of the data, +perhaps made up of 10 images. A 700-image dataset may take perhaps 6 +hours on a machine with a couple of dozen cores and use up perhaps 20 +GB of RAM. + +The obtained map can be examined with ``nvm_visualize``, as described +in \ref sparsemapping. + +The work directory can be deleted later. + +# Command line options This tool has the following command-line options: - --theia_flags: The flags to pass to Theia. If not specified, use - localization/sparse_mapping/theia_flags.txt in the Astrobee repo. - --image_list: The list of distorted (original) nav cam images to - use, one per line. - --output_map: The resulting output map. - --skip_rebuilding: Do not rebuild the map after importing it from - Theia. - --work_dir: A temporary work directory to be deleted by the user - later. - --keep_undistorted_images: Do not replace the undistorted images - Theia used with the original distorted ones in the sparse map - imported from Theia. This is for testing purposes. - --help: Show this help message and exit. + --theia_flags: The flags to pass to Theia. If not specified, use + localization/sparse_mapping/theia_flags.txt in the Astrobee repo. + --image_list: The list of distorted (original) nav cam images to + use, one per line. + --output_map: The resulting output map. + --skip_rebuilding: Do not rebuild the map after importing it from + Theia. + --work_dir: A temporary work directory to be deleted by the user + later. + --keep_undistorted_images: Do not replace the undistorted images + Theia used with the original distorted ones in the sparse map + imported from Theia. This is for testing purposes. + --help: Show this help message and exit. + +# Next steps + +This map will need to be registered and visualized as described in +\ref map_building. + +That page also has information for how the map can be rebuilt to use +BRISK features, and how it can be validated for localization by +playing a bag against it. # Auxiliary import_map tool @@ -114,7 +210,7 @@ exports to. These operations are done automatically by the reference only. An NVM map exported by Theia (or some other SfM tool) can be saved as -an Astrobee sparse map with the command:: +an Astrobee sparse map with the command: astrobee/devel/lib/sparse_mapping/import_map \ -undistorted_camera_params "wid_x wid_y focal_len opt_ctr_x opt_ctr_y" \ @@ -129,15 +225,15 @@ should be as printed on the screen (and saved to disk) by If desired to replace on importing the undistorted images with the original distorted ones, as it is usually expected of a sparse map, -the above command should be called instead as:: +the above command should be called instead as: astrobee/devel/lib/sparse_mapping/import_map \ -undistorted_images_list undist_list.txt \ -distorted_images_list dist_list.txt \ -input_map map.nvm -output_map map.map -Here, the files undist_list.txt and dist_list.txt must have one image -per line and be in one-to-one correspondence. It is important that -both undistorted and distorted images be specified, as the former are -needed to look up camera poses and other data in the .nvm file before -being replaced with the distorted ones. +Here, the files ``undist_list.txt`` and ``dist_list.txt`` must have +one image per line and be in one-to-one correspondence. It is +important that both undistorted and distorted images be specified, as +the former are needed to look up camera poses and other data in the +.nvm file before being replaced with the distorted ones. diff --git a/localization/sparse_mapping/tools/build_map.cc b/localization/sparse_mapping/tools/build_map.cc index 8dadb16eac..69663811a1 100644 --- a/localization/sparse_mapping/tools/build_map.cc +++ b/localization/sparse_mapping/tools/build_map.cc @@ -453,6 +453,12 @@ int main(int argc, char** argv) { ff_common::InitFreeFlyerApplication(&argc, &argv); GOOGLE_PROTOBUF_VERIFY_VERSION; + // It is important to get the robot name right + char * bot = getenv("ASTROBEE_ROBOT"); + if (bot == NULL) + LOG(FATAL) << "ASTROBEE_ROBOT was not set.\n"; + std::cout << "ASTROBEE_ROBOT=" << bot << std::endl; + if (FLAGS_output_map == "") LOG(FATAL) << "Must specify the output map name."; diff --git a/localization/sparse_mapping/tools/nvm_visualize.cc b/localization/sparse_mapping/tools/nvm_visualize.cc index c4b0990426..a770221737 100644 --- a/localization/sparse_mapping/tools/nvm_visualize.cc +++ b/localization/sparse_mapping/tools/nvm_visualize.cc @@ -617,7 +617,11 @@ int main(int argc, char** argv) { cid = num_images - 1; // The 'End' key was pressed, go to the last image } - if ((ret =='x' || ret == 255) && FLAGS_enable_image_deletion) { + // Note that the 'Delete' key is not used, because in some + // situations, due to some quirk, certain window events can send a + // signal the viewer interprets as this key, and images are + // deleted then when not intended. + if (ret =='x' && FLAGS_enable_image_deletion) { deleteImageFromDiskAndMap(map, imfile); num_images = map.GetNumFrames(); // update the number of images if (num_images == 0) { diff --git a/localization/vision_common/CMakeLists.txt b/localization/vision_common/CMakeLists.txt new file mode 100644 index 0000000000..fe3e704256 --- /dev/null +++ b/localization/vision_common/CMakeLists.txt @@ -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. + +cmake_minimum_required(VERSION 3.0) +project(vision_common) + +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + localization_common optimization_common +) + +# Find OpenCV3 +SET(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/../../cmake") +find_package(OpenCV331 REQUIRED) + +find_package(Eigen3 REQUIRED) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} ${OpenCV_LIBRARIES} + CATKIN_DEPENDS localization_common optimization_common +) + +########### +## Build ## +########### +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} +) + +# Declare C++ libraries +add_library(${PROJECT_NAME} + src/brisk_feature_detector_and_matcher.cc + src/camera_utilities.cc + src/good_features_to_track_detector.cc + src/lk_optical_flow_feature_detector_and_matcher.cc + src/parameter_reader.cc + src/surf_feature_detector_and_matcher.cc + src/test_utilities.cc +) +add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest_gtest(test_lk_optical_flow_feature_detector_and_matcher + test/test_lk_optical_flow_feature_detector_and_matcher.test + test/test_lk_optical_flow_feature_detector_and_matcher.cc + ) + target_link_libraries(test_lk_optical_flow_feature_detector_and_matcher + ${PROJECT_NAME} ${catkin_LIBRARIES} + ) + find_package(rostest REQUIRED) + add_rostest_gtest(test_ransac_pnp + test/test_ransac_pnp.test + test/test_ransac_pnp.cc + ) + target_link_libraries(test_ransac_pnp + ${PROJECT_NAME} ${catkin_LIBRARIES} + ) + add_rostest_gtest(test_reprojection_pose_estimate + test/test_reprojection_pose_estimate.test + test/test_reprojection_pose_estimate.cc + ) + target_link_libraries(test_reprojection_pose_estimate + ${PROJECT_NAME} ${catkin_LIBRARIES} + ) + add_rostest_gtest(test_pnp + test/test_pnp.test + test/test_pnp.cc + ) + target_link_libraries(test_pnp + ${PROJECT_NAME} ${catkin_LIBRARIES} + ) + add_rostest_gtest(test_camera_utilities + test/test_camera_utilities.test + test/test_camera_utilities.cc + ) + target_link_libraries(test_camera_utilities + ${PROJECT_NAME} ${catkin_LIBRARIES} + ) + +endif() + +############# +## Install ## +############# + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) diff --git a/localization/vision_common/include/vision_common/brisk_feature_detector_and_matcher.h b/localization/vision_common/include/vision_common/brisk_feature_detector_and_matcher.h new file mode 100644 index 0000000000..b3d0036432 --- /dev/null +++ b/localization/vision_common/include/vision_common/brisk_feature_detector_and_matcher.h @@ -0,0 +1,37 @@ +/* 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. + */ +#ifndef VISION_COMMON_BRISK_FEATURE_DETECTOR_AND_MATCHER_H_ +#define VISION_COMMON_BRISK_FEATURE_DETECTOR_AND_MATCHER_H_ + +#include +#include +#include + +namespace vision_common { +class BriskFeatureDetectorAndMatcher : public FeatureDetectorAndMatcher { + public: + BriskFeatureDetectorAndMatcher(const BriskFeatureDetectorAndMatcherParams& params); + FeatureMatches Match(const FeatureImage& source_image, const FeatureImage& target_image) final; + + private: + BriskFeatureDetectorAndMatcherParams params_; + cv::FlannBasedMatcher flann_matcher_; +}; +} // namespace vision_common + +#endif // VISION_COMMON_BRISK_FEATURE_DETECTOR_AND_MATCHER_H_ diff --git a/localization/vision_common/include/vision_common/brisk_feature_detector_and_matcher_params.h b/localization/vision_common/include/vision_common/brisk_feature_detector_and_matcher_params.h new file mode 100644 index 0000000000..766f39d96d --- /dev/null +++ b/localization/vision_common/include/vision_common/brisk_feature_detector_and_matcher_params.h @@ -0,0 +1,35 @@ +/* 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. + */ +#ifndef VISION_COMMON_BRISK_FEATURE_DETECTOR_AND_MATCHER_PARAMS_H_ +#define VISION_COMMON_BRISK_FEATURE_DETECTOR_AND_MATCHER_PARAMS_H_ + +namespace vision_common { +struct BriskFeatureDetectorAndMatcherParams { + // Detection + int brisk_threshold; + int brisk_octaves; + float brisk_float_pattern_scale; + // Matching + int max_match_hamming_distance; + int flann_table_number; + int flann_key_size; + int flann_multi_probe_level; +}; +} // namespace vision_common + +#endif // VISION_COMMON_BRISK_FEATURE_DETECTOR_AND_MATCHER_PARAMS_H_ diff --git a/tools/calibration/include/calibration/camera_utilities.h b/localization/vision_common/include/vision_common/camera_utilities.h similarity index 98% rename from tools/calibration/include/calibration/camera_utilities.h rename to localization/vision_common/include/vision_common/camera_utilities.h index 777f5657e8..135fb8dde3 100644 --- a/tools/calibration/include/calibration/camera_utilities.h +++ b/localization/vision_common/include/vision_common/camera_utilities.h @@ -15,18 +15,18 @@ * License for the specific language governing permissions and limitations * under the License. */ -#ifndef CALIBRATION_CAMERA_UTILITIES_H_ -#define CALIBRATION_CAMERA_UTILITIES_H_ +#ifndef VISION_COMMON_CAMERA_UTILITIES_H_ +#define VISION_COMMON_CAMERA_UTILITIES_H_ -#include -#include -#include #include #include #include #include #include #include +#include +#include +#include #include @@ -47,7 +47,7 @@ #include #include -namespace calibration { +namespace vision_common { Eigen::Vector2d Project3dPointToImageSpace(const Eigen::Vector3d& cam_t_point, const Eigen::Matrix3d& intrinsics); template @@ -336,5 +336,5 @@ boost::optional ReprojectionPoseEstimateWithInitia return ReprojectionPoseEstimateWithInitialEstimate( image_points, points_3d, focal_lengths, principal_points, distortion, params, initial_estimate, initial_inliers); } -} // namespace calibration -#endif // CALIBRATION_CAMERA_UTILITIES_H_ +} // namespace vision_common +#endif // VISION_COMMON_CAMERA_UTILITIES_H_ diff --git a/localization/optimization_common/include/optimization_common/distorter.h b/localization/vision_common/include/vision_common/distorter.h similarity index 91% rename from localization/optimization_common/include/optimization_common/distorter.h rename to localization/vision_common/include/vision_common/distorter.h index 880a8c8272..2ddce8f0af 100644 --- a/localization/optimization_common/include/optimization_common/distorter.h +++ b/localization/vision_common/include/vision_common/distorter.h @@ -15,8 +15,8 @@ * License for the specific language governing permissions and limitations * under the License. */ -#ifndef OPTIMIZATION_COMMON_DISTORTER_H_ -#define OPTIMIZATION_COMMON_DISTORTER_H_ +#ifndef VISION_COMMON_DISTORTER_H_ +#define VISION_COMMON_DISTORTER_H_ #include @@ -24,7 +24,7 @@ #include -namespace optimization_common { +namespace vision_common { template class Distorter { public: @@ -50,6 +50,6 @@ class Distorter { static constexpr int kNumParams = NUM_PARAMS; }; -} // namespace optimization_common +} // namespace vision_common -#endif // OPTIMIZATION_COMMON_DISTORTER_H_ +#endif // VISION_COMMON_DISTORTER_H_ diff --git a/localization/vision_common/include/vision_common/feature_detector_and_matcher.h b/localization/vision_common/include/vision_common/feature_detector_and_matcher.h new file mode 100644 index 0000000000..0cd3084ddd --- /dev/null +++ b/localization/vision_common/include/vision_common/feature_detector_and_matcher.h @@ -0,0 +1,37 @@ +/* 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. + */ +#ifndef VISION_COMMON_FEATURE_DETECTOR_AND_MATCHER_H_ +#define VISION_COMMON_FEATURE_DETECTOR_AND_MATCHER_H_ + +#include +#include + +#include + +namespace vision_common { +class FeatureDetectorAndMatcher { + public: + virtual FeatureMatches Match(const FeatureImage& source_image, const FeatureImage& target_image) = 0; + const cv::Ptr& detector() { return detector_; } + + protected: + cv::Ptr detector_; +}; +} // namespace vision_common + +#endif // VISION_COMMON_FEATURE_DETECTOR_AND_MATCHER_H_ diff --git a/localization/vision_common/include/vision_common/feature_image.h b/localization/vision_common/include/vision_common/feature_image.h new file mode 100644 index 0000000000..b039412ef5 --- /dev/null +++ b/localization/vision_common/include/vision_common/feature_image.h @@ -0,0 +1,48 @@ +/* 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. + */ +#ifndef VISION_COMMON_FEATURE_IMAGE_H_ +#define VISION_COMMON_FEATURE_IMAGE_H_ + +#include +#include + +#include + +namespace vision_common { +class FeatureImage { + public: + FeatureImage(const cv::Mat& image, cv::Feature2D& feature_detector) : image_(image) { + feature_detector.detectAndCompute(image_, cv::Mat(), keypoints_, descriptors_); + cv::KeyPoint::convert(keypoints_, feature_points_); + } + const cv::Mat& image() const { return image_; } + const int rows() const { return image_.rows; } + const int cols() const { return image_.cols; } + const std::vector& keypoints() const { return keypoints_; } + const std::vector& feature_points() const { return feature_points_; } + const cv::Mat& descriptors() const { return descriptors_; } + + private: + cv::Mat image_; + std::vector keypoints_; + std::vector feature_points_; + cv::Mat descriptors_; +}; +} // namespace vision_common + +#endif // VISION_COMMON_FEATURE_IMAGE_H_ diff --git a/localization/vision_common/include/vision_common/feature_match.h b/localization/vision_common/include/vision_common/feature_match.h new file mode 100644 index 0000000000..9b9de7dd07 --- /dev/null +++ b/localization/vision_common/include/vision_common/feature_match.h @@ -0,0 +1,38 @@ +/* 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. + */ +#ifndef VISION_COMMON_FEATURE_MATCH_H_ +#define VISION_COMMON_FEATURE_MATCH_H_ + +#include + +#include + +namespace vision_common { +struct FeatureMatch { + FeatureMatch(const Eigen::Vector2d& source_point, const Eigen::Vector2d& target_point, const double distance) + : source_point(source_point), target_point(target_point), distance(distance) {} + Eigen::Vector2d source_point; + Eigen::Vector2d target_point; + double distance; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; +using FeatureMatches = std::vector; +} // namespace vision_common + +#endif // VISION_COMMON_FEATURE_MATCH_H_ diff --git a/localization/optimization_common/include/optimization_common/fov_distorter.h b/localization/vision_common/include/vision_common/fov_distorter.h similarity index 93% rename from localization/optimization_common/include/optimization_common/fov_distorter.h rename to localization/vision_common/include/vision_common/fov_distorter.h index b9d1ef8a97..2497180d05 100644 --- a/localization/optimization_common/include/optimization_common/fov_distorter.h +++ b/localization/vision_common/include/vision_common/fov_distorter.h @@ -15,11 +15,11 @@ * License for the specific language governing permissions and limitations * under the License. */ -#ifndef OPTIMIZATION_COMMON_FOV_DISTORTER_H_ -#define OPTIMIZATION_COMMON_FOV_DISTORTER_H_ +#ifndef VISION_COMMON_FOV_DISTORTER_H_ +#define VISION_COMMON_FOV_DISTORTER_H_ -#include -#include +#include +#include #include @@ -27,7 +27,7 @@ #include -namespace optimization_common { +namespace vision_common { class FovDistorter : public Distorter<1, FovDistorter> { public: using Distorter<1, FovDistorter>::Distort; @@ -88,6 +88,6 @@ class FovDistorter : public Distorter<1, FovDistorter> { return undistorted_point; } }; -} // namespace optimization_common +} // namespace vision_common -#endif // OPTIMIZATION_COMMON_FOV_DISTORTER_H_ +#endif // VISION_COMMON_FOV_DISTORTER_H_ diff --git a/localization/vision_common/include/vision_common/good_features_to_track_detector.h b/localization/vision_common/include/vision_common/good_features_to_track_detector.h new file mode 100644 index 0000000000..9a1940008c --- /dev/null +++ b/localization/vision_common/include/vision_common/good_features_to_track_detector.h @@ -0,0 +1,40 @@ +/* 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. + */ +#ifndef VISION_COMMON_GOOD_FEATURES_TO_TRACK_DETECTOR_H_ +#define VISION_COMMON_GOOD_FEATURES_TO_TRACK_DETECTOR_H_ + +#include + +#include + +#include + +namespace cv { +class GoodFeaturesToTrackDetector : public cv::Feature2D { + public: + explicit GoodFeaturesToTrackDetector(const vision_common::GoodFeaturesToTrackDetectorParams& params); + ~GoodFeaturesToTrackDetector() final {} + void detectAndCompute(cv::InputArray image, cv::InputArray mask, std::vector& keypoints, + cv::OutputArray descriptors, bool useProvidedKeypoints = false) final; + + private: + vision_common::GoodFeaturesToTrackDetectorParams params_; +}; +} // namespace cv + +#endif // VISION_COMMON_GOOD_FEATURES_TO_TRACK_DETECTOR_H_ diff --git a/localization/vision_common/include/vision_common/good_features_to_track_detector_params.h b/localization/vision_common/include/vision_common/good_features_to_track_detector_params.h new file mode 100644 index 0000000000..47057033ee --- /dev/null +++ b/localization/vision_common/include/vision_common/good_features_to_track_detector_params.h @@ -0,0 +1,32 @@ +/* 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. + */ +#ifndef VISION_COMMON_GOOD_FEATURES_TO_TRACK_DETECTOR_PARAMS_H_ +#define VISION_COMMON_GOOD_FEATURES_TO_TRACK_DETECTOR_PARAMS_H_ + +namespace vision_common { +struct GoodFeaturesToTrackDetectorParams { + int max_corners; + double quality_level; + double min_distance; + int block_size; + bool use_harris_detector; + double k; +}; +} // namespace vision_common + +#endif // VISION_COMMON_GOOD_FEATURES_TO_TRACK_DETECTOR_PARAMS_H_ diff --git a/localization/optimization_common/include/optimization_common/identity_distorter.h b/localization/vision_common/include/vision_common/identity_distorter.h similarity index 86% rename from localization/optimization_common/include/optimization_common/identity_distorter.h rename to localization/vision_common/include/vision_common/identity_distorter.h index 45962298e4..6c45627be0 100644 --- a/localization/optimization_common/include/optimization_common/identity_distorter.h +++ b/localization/vision_common/include/vision_common/identity_distorter.h @@ -15,12 +15,12 @@ * License for the specific language governing permissions and limitations * under the License. */ -#ifndef OPTIMIZATION_COMMON_IDENTITY_DISTORTER_H_ -#define OPTIMIZATION_COMMON_IDENTITY_DISTORTER_H_ +#ifndef VISION_COMMON_IDENTITY_DISTORTER_H_ +#define VISION_COMMON_IDENTITY_DISTORTER_H_ -#include +#include -namespace optimization_common { +namespace vision_common { // TODO(rsoussan): This actually takes 0 params but ceres needs at least 1 to form // valid parameter blocks. Change to 0 when this is avoided. class IdentityDistorter : public Distorter<1, IdentityDistorter> { @@ -44,6 +44,6 @@ class IdentityDistorter : public Distorter<1, IdentityDistorter> { return distorted_point; } }; -} // namespace optimization_common +} // namespace vision_common -#endif // OPTIMIZATION_COMMON_IDENTITY_DISTORTER_H_ +#endif // VISION_COMMON_IDENTITY_DISTORTER_H_ diff --git a/localization/vision_common/include/vision_common/lk_optical_flow_feature_detector_and_matcher.h b/localization/vision_common/include/vision_common/lk_optical_flow_feature_detector_and_matcher.h new file mode 100644 index 0000000000..97b618866b --- /dev/null +++ b/localization/vision_common/include/vision_common/lk_optical_flow_feature_detector_and_matcher.h @@ -0,0 +1,36 @@ +/* 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. + */ +#ifndef VISION_COMMON_LK_OPTICAL_FLOW_FEATURE_DETECTOR_AND_MATCHER_H_ +#define VISION_COMMON_LK_OPTICAL_FLOW_FEATURE_DETECTOR_AND_MATCHER_H_ + +#include +#include +#include + +namespace vision_common { +class LKOpticalFlowFeatureDetectorAndMatcher : public FeatureDetectorAndMatcher { + public: + explicit LKOpticalFlowFeatureDetectorAndMatcher(const LKOpticalFlowFeatureDetectorAndMatcherParams& params); + FeatureMatches Match(const FeatureImage& source_image, const FeatureImage& target_image) final; + + private: + LKOpticalFlowFeatureDetectorAndMatcherParams params_; +}; +} // namespace vision_common + +#endif // VISION_COMMON_LK_OPTICAL_FLOW_FEATURE_DETECTOR_AND_MATCHER_H_ diff --git a/localization/vision_common/include/vision_common/lk_optical_flow_feature_detector_and_matcher_params.h b/localization/vision_common/include/vision_common/lk_optical_flow_feature_detector_and_matcher_params.h new file mode 100644 index 0000000000..01c4bf2bcc --- /dev/null +++ b/localization/vision_common/include/vision_common/lk_optical_flow_feature_detector_and_matcher_params.h @@ -0,0 +1,36 @@ +/* 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. + */ +#ifndef VISION_COMMON_LK_OPTICAL_FLOW_FEATURE_DETECTOR_AND_MATCHER_PARAMS_H_ +#define VISION_COMMON_LK_OPTICAL_FLOW_FEATURE_DETECTOR_AND_MATCHER_PARAMS_H_ + +#include + +namespace vision_common { +struct LKOpticalFlowFeatureDetectorAndMatcherParams { + int max_iterations; + double termination_epsilon; + int window_length; + int max_level; + double min_eigen_threshold; + double max_flow_distance; + double max_backward_match_distance; + GoodFeaturesToTrackDetectorParams good_features_to_track; +}; +} // namespace vision_common + +#endif // VISION_COMMON_LK_OPTICAL_FLOW_FEATURE_DETECTOR_AND_MATCHER_PARAMS_H_ diff --git a/localization/vision_common/include/vision_common/parameter_reader.h b/localization/vision_common/include/vision_common/parameter_reader.h new file mode 100644 index 0000000000..f757fde3bc --- /dev/null +++ b/localization/vision_common/include/vision_common/parameter_reader.h @@ -0,0 +1,37 @@ +/* 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. + */ +#ifndef VISION_COMMON_PARAMETER_READER_H_ +#define VISION_COMMON_PARAMETER_READER_H_ + +#include +#include +#include +#include +#include + +namespace vision_common { +void LoadBriskFeatureDetectorAndMatcherParams(config_reader::ConfigReader& config, + BriskFeatureDetectorAndMatcherParams& params); +void LoadGoodFeaturesToTrackDetectorParams(config_reader::ConfigReader& config, + GoodFeaturesToTrackDetectorParams& params); +void LoadLKOpticalFlowFeatureDetectorAndMatcherParams(config_reader::ConfigReader& config, + LKOpticalFlowFeatureDetectorAndMatcherParams& params); +void LoadSurfFeatureDetectorAndMatcherParams(config_reader::ConfigReader& config, + SurfFeatureDetectorAndMatcherParams& params); +} // namespace vision_common +#endif // VISION_COMMON_PARAMETER_READER_H_ diff --git a/tools/calibration/include/calibration/pose_with_covariance_and_inliers.h b/localization/vision_common/include/vision_common/pose_with_covariance_and_inliers.h similarity index 84% rename from tools/calibration/include/calibration/pose_with_covariance_and_inliers.h rename to localization/vision_common/include/vision_common/pose_with_covariance_and_inliers.h index 3a510b7fe2..a18c6faaae 100644 --- a/tools/calibration/include/calibration/pose_with_covariance_and_inliers.h +++ b/localization/vision_common/include/vision_common/pose_with_covariance_and_inliers.h @@ -15,8 +15,8 @@ * License for the specific language governing permissions and limitations * under the License. */ -#ifndef CALIBRATION_POSE_WITH_COVARIANCE_AND_INLIERS_H_ -#define CALIBRATION_POSE_WITH_COVARIANCE_AND_INLIERS_H_ +#ifndef VISION_COMMON_POSE_WITH_COVARIANCE_AND_INLIERS_H_ +#define VISION_COMMON_POSE_WITH_COVARIANCE_AND_INLIERS_H_ #include @@ -24,13 +24,13 @@ #include -namespace calibration { +namespace vision_common { struct PoseWithCovarianceAndInliers : public localization_common::PoseWithCovariance { PoseWithCovarianceAndInliers(const Eigen::Isometry3d& pose, const localization_common::PoseCovariance& covariance, const std::vector& inliers) : PoseWithCovariance(pose, covariance), inliers(inliers) {} std::vector inliers; }; -} // namespace calibration +} // namespace vision_common -#endif // CALIBRATION_POSE_WITH_COVARIANCE_AND_INLIERS_H_ +#endif // VISION_COMMON_POSE_WITH_COVARIANCE_AND_INLIERS_H_ diff --git a/localization/optimization_common/include/optimization_common/rad_distorter.h b/localization/vision_common/include/vision_common/rad_distorter.h similarity index 88% rename from localization/optimization_common/include/optimization_common/rad_distorter.h rename to localization/vision_common/include/vision_common/rad_distorter.h index 7b2b94f6dc..f21eda39bc 100644 --- a/localization/optimization_common/include/optimization_common/rad_distorter.h +++ b/localization/vision_common/include/vision_common/rad_distorter.h @@ -15,18 +15,17 @@ * License for the specific language governing permissions and limitations * under the License. */ -#ifndef OPTIMIZATION_COMMON_RAD_DISTORTER_H_ -#define OPTIMIZATION_COMMON_RAD_DISTORTER_H_ +#ifndef VISION_COMMON_RAD_DISTORTER_H_ +#define VISION_COMMON_RAD_DISTORTER_H_ -#include -#include -#include +#include +#include #include #include -namespace optimization_common { +namespace vision_common { class RadDistorter : public Distorter<2, RadDistorter> { public: using Distorter<2, RadDistorter>::Distort; @@ -67,6 +66,6 @@ class RadDistorter : public Distorter<2, RadDistorter> { RadTanDistorter radtan_distorter_; }; -} // namespace optimization_common +} // namespace vision_common -#endif // OPTIMIZATION_COMMON_RAD_DISTORTER_H_ +#endif // VISION_COMMON_RAD_DISTORTER_H_ diff --git a/localization/optimization_common/include/optimization_common/radtan_distorter.h b/localization/vision_common/include/vision_common/radtan_distorter.h similarity index 92% rename from localization/optimization_common/include/optimization_common/radtan_distorter.h rename to localization/vision_common/include/vision_common/radtan_distorter.h index 37c97c280c..b0a6d10192 100644 --- a/localization/optimization_common/include/optimization_common/radtan_distorter.h +++ b/localization/vision_common/include/vision_common/radtan_distorter.h @@ -15,11 +15,11 @@ * License for the specific language governing permissions and limitations * under the License. */ -#ifndef OPTIMIZATION_COMMON_RADTAN_DISTORTER_H_ -#define OPTIMIZATION_COMMON_RADTAN_DISTORTER_H_ +#ifndef VISION_COMMON_RADTAN_DISTORTER_H_ +#define VISION_COMMON_RADTAN_DISTORTER_H_ -#include -#include +#include +#include #include @@ -27,7 +27,7 @@ #include -namespace optimization_common { +namespace vision_common { class RadTanDistorter : public Distorter<4, RadTanDistorter> { public: using Distorter<4, RadTanDistorter>::Distort; @@ -89,6 +89,6 @@ class RadTanDistorter : public Distorter<4, RadTanDistorter> { return undistorted_point; } }; -} // namespace optimization_common +} // namespace vision_common -#endif // OPTIMIZATION_COMMON_RADTAN_DISTORTER_H_ +#endif // VISION_COMMON_RADTAN_DISTORTER_H_ diff --git a/tools/calibration/include/calibration/ransac_pnp_params.h b/localization/vision_common/include/vision_common/ransac_pnp_params.h similarity index 83% rename from tools/calibration/include/calibration/ransac_pnp_params.h rename to localization/vision_common/include/vision_common/ransac_pnp_params.h index 515738cc89..4c7f5e20f2 100644 --- a/tools/calibration/include/calibration/ransac_pnp_params.h +++ b/localization/vision_common/include/vision_common/ransac_pnp_params.h @@ -15,12 +15,12 @@ * License for the specific language governing permissions and limitations * under the License. */ -#ifndef CALIBRATION_RANSAC_PNP_PARAMS_H_ -#define CALIBRATION_RANSAC_PNP_PARAMS_H_ +#ifndef VISION_COMMON_RANSAC_PNP_PARAMS_H_ +#define VISION_COMMON_RANSAC_PNP_PARAMS_H_ #include -namespace calibration { +namespace vision_common { struct RansacPnPParams { double max_inlier_threshold; int num_iterations; @@ -28,6 +28,6 @@ struct RansacPnPParams { // TODO(rsoussan): Change this to cv::SolvePnPMethod when opencv version updated int pnp_method; }; -} // namespace calibration +} // namespace vision_common -#endif // CALIBRATION_RANSAC_PNP_PARAMS_H_ +#endif // VISION_COMMON_RANSAC_PNP_PARAMS_H_ diff --git a/tools/calibration/include/calibration/reprojection_pose_estimate_params.h b/localization/vision_common/include/vision_common/reprojection_pose_estimate_params.h similarity index 69% rename from tools/calibration/include/calibration/reprojection_pose_estimate_params.h rename to localization/vision_common/include/vision_common/reprojection_pose_estimate_params.h index f7ebe47175..35da8b4532 100644 --- a/tools/calibration/include/calibration/reprojection_pose_estimate_params.h +++ b/localization/vision_common/include/vision_common/reprojection_pose_estimate_params.h @@ -15,19 +15,19 @@ * License for the specific language governing permissions and limitations * under the License. */ -#ifndef CALIBRATION_REPROJECTION_POSE_ESTIMATE_PARAMS_H_ -#define CALIBRATION_REPROJECTION_POSE_ESTIMATE_PARAMS_H_ +#ifndef VISION_COMMON_REPROJECTION_POSE_ESTIMATE_PARAMS_H_ +#define VISION_COMMON_REPROJECTION_POSE_ESTIMATE_PARAMS_H_ -#include -#include +#include +#include -namespace calibration { +namespace vision_common { struct ReprojectionPoseEstimateParams { - OptimizationParams optimization; + optimization_common::OptimizationParams optimization; RansacPnPParams ransac_pnp; bool optimize_estimate; double max_inlier_threshold; }; -} // namespace calibration +} // namespace vision_common -#endif // CALIBRATION_REPROJECTION_POSE_ESTIMATE_PARAMS_H_ +#endif // VISION_COMMON_REPROJECTION_POSE_ESTIMATE_PARAMS_H_ diff --git a/localization/vision_common/include/vision_common/surf_feature_detector_and_matcher.h b/localization/vision_common/include/vision_common/surf_feature_detector_and_matcher.h new file mode 100644 index 0000000000..da11d7fa07 --- /dev/null +++ b/localization/vision_common/include/vision_common/surf_feature_detector_and_matcher.h @@ -0,0 +1,37 @@ +/* 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. + */ +#ifndef VISION_COMMON_SURF_FEATURE_DETECTOR_AND_MATCHER_H_ +#define VISION_COMMON_SURF_FEATURE_DETECTOR_AND_MATCHER_H_ + +#include +#include +#include + +namespace vision_common { +class SurfFeatureDetectorAndMatcher : public FeatureDetectorAndMatcher { + public: + explicit SurfFeatureDetectorAndMatcher(const SurfFeatureDetectorAndMatcherParams& params); + FeatureMatches Match(const FeatureImage& source_image, const FeatureImage& target_image) final; + + private: + SurfFeatureDetectorAndMatcherParams params_; + cv::FlannBasedMatcher flann_matcher_; +}; +} // namespace vision_common + +#endif // VISION_COMMON_SURF_FEATURE_DETECTOR_AND_MATCHER_H_ diff --git a/localization/vision_common/include/vision_common/surf_feature_detector_and_matcher_params.h b/localization/vision_common/include/vision_common/surf_feature_detector_and_matcher_params.h new file mode 100644 index 0000000000..bff52e8e36 --- /dev/null +++ b/localization/vision_common/include/vision_common/surf_feature_detector_and_matcher_params.h @@ -0,0 +1,28 @@ +/* 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. + */ +#ifndef VISION_COMMON_SURF_FEATURE_DETECTOR_AND_MATCHER_PARAMS_H_ +#define VISION_COMMON_SURF_FEATURE_DETECTOR_AND_MATCHER_PARAMS_H_ + +namespace vision_common { +struct SurfFeatureDetectorAndMatcherParams { + int surf_threshold; + double max_match_distance; +}; +} // namespace vision_common + +#endif // VISION_COMMON_SURF_FEATURE_DETECTOR_AND_MATCHER_PARAMS_H_ diff --git a/localization/vision_common/include/vision_common/test_utilities.h b/localization/vision_common/include/vision_common/test_utilities.h new file mode 100644 index 0000000000..f1f9435529 --- /dev/null +++ b/localization/vision_common/include/vision_common/test_utilities.h @@ -0,0 +1,113 @@ +/* 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. + */ +#ifndef VISION_COMMON_TEST_UTILITIES_H_ +#define VISION_COMMON_TEST_UTILITIES_H_ + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +namespace vision_common { +LKOpticalFlowFeatureDetectorAndMatcherParams DefaultLKOpticalFlowFeatureDetectorAndMatcherParams(); + +cv::Mat MarkerImage(const int row_spacing, const int col_spacing, int& num_markers_added, + const cv::Point2i& offset = cv::Point2i(0, 0)); + +int AddMarkers(const int row_spacing, const int col_spacing, cv::Mat& image, + const cv::Point2i& offset = cv::Point2i(0, 0)); + +optimization_common::OptimizationParams DefaultOptimizationParams(); + +RansacPnPParams DefaultRansacPnPParams(); + +ReprojectionPoseEstimateParams DefaultReprojectionPoseEstimateParams(); + +Eigen::VectorXd RandomFovDistortion(); + +Eigen::VectorXd RandomRadDistortion(); + +Eigen::VectorXd RandomRadTanDistortion(); + +// Samples in cylindrical coordinates for pose translation to keep pose in view frustrum. +// Samples z using scaled rho value to prevent large z vals with small rho values +// that may move the pose out of the view frustrum. +Eigen::Isometry3d RandomFrontFacingPose(const double rho_min, const double rho_max, const double phi_min, + const double phi_max, const double z_rho_scale, const double yaw_min, + const double yaw_max, const double pitch_min, const double pitch_max, + const double roll_min, const double roll_max); + +Eigen::Isometry3d RandomFrontFacingPose(); + +// Spaced out poses for targets which when projected into image space cover +// the image well with target points. Poses are sampled for each row/col combination +// and evenly spaced in cylindrical coordinates +std::vector EvenlySpacedTargetPoses(const int num_rows = 3, const int num_cols = 5, + const int num_y_levels = 2); + +std::vector TargetPoints(const int points_per_row, const int points_per_col, + const double row_spacing = 0.1, const double col_spacing = 0.1); + +std::vector RandomFrontFacingPoints(const int num_points); + +Eigen::Vector3d RandomFrontFacingPoint(); + +Eigen::Vector3d RandomFrontFacingPoint(const double rho_min, const double rho_max, const double phi_min, + const double phi_max, const double z_rho_scale); + +template +class RegistrationCorrespondences { + public: + RegistrationCorrespondences(const Eigen::Isometry3d& camera_T_target, const Eigen::Matrix3d& intrinsics, + const std::vector& target_t_target_point, + const Eigen::VectorXd& distortion = Eigen::VectorXd()); + + const localization_common::ImageCorrespondences& correspondences() const { return correspondences_; } + + const Eigen::Isometry3d& camera_T_target() const { return camera_T_target_; } + + const Eigen::Matrix3d& intrinsics() const { return intrinsics_; } + + private: + localization_common::ImageCorrespondences correspondences_; + Eigen::Isometry3d camera_T_target_; + Eigen::Matrix3d intrinsics_; +}; + +template +RegistrationCorrespondences::RegistrationCorrespondences( + const Eigen::Isometry3d& camera_T_target, const Eigen::Matrix3d& intrinsics, + const std::vector& target_t_target_points, const Eigen::VectorXd& distortion) + : camera_T_target_(camera_T_target), intrinsics_(intrinsics) { + for (const auto& target_t_target_point : target_t_target_points) { + const Eigen::Vector3d camera_t_target_point = camera_T_target_ * target_t_target_point; + if (camera_t_target_point.z() <= 0) continue; + const Eigen::Vector2d image_point = + Project3dPointToImageSpaceWithDistortion(camera_t_target_point, intrinsics_, distortion); + correspondences_.AddCorrespondence(image_point, target_t_target_point); + } +} +} // namespace vision_common +#endif // VISION_COMMON_TEST_UTILITIES_H_ diff --git a/localization/vision_common/include/vision_common/utilities.h b/localization/vision_common/include/vision_common/utilities.h new file mode 100644 index 0000000000..e0a2e1660e --- /dev/null +++ b/localization/vision_common/include/vision_common/utilities.h @@ -0,0 +1,58 @@ +/* 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. + */ +#ifndef VISION_COMMON_UTILITIES_H_ +#define VISION_COMMON_UTILITIES_H_ + +#include + +namespace vision_common { +template +Eigen::Matrix RelativeCoordinates(const Eigen::Matrix& absolute_point, + const Eigen::Matrix& intrinsics); +template +Eigen::Matrix AbsoluteCoordinates(const Eigen::Matrix& relative_point, + const Eigen::Matrix& intrinsics); + +// Implementation +template +Eigen::Matrix RelativeCoordinates(const Eigen::Matrix& absolute_point, + const Eigen::Matrix& intrinsics) { + const T& f_x = intrinsics(0, 0); + const T& f_y = intrinsics(1, 1); + const T& p_x = intrinsics(0, 2); + const T& p_y = intrinsics(1, 2); + + const T& x = absolute_point[0]; + const T& y = absolute_point[1]; + const T relative_x = (x - p_x) / f_x; + const T relative_y = (y - p_y) / f_y; + return Eigen::Matrix(relative_x, relative_y); +} + +template +Eigen::Matrix AbsoluteCoordinates(const Eigen::Matrix& relative_point, + const Eigen::Matrix& intrinsics) { + const T& f_x = intrinsics(0, 0); + const T& f_y = intrinsics(1, 1); + const T& p_x = intrinsics(0, 2); + const T& p_y = intrinsics(1, 2); + return Eigen::Matrix(relative_point[0] * f_x + p_x, relative_point[1] * f_y + p_y); +} +} // namespace vision_common + +#endif // VISION_COMMON_UTILITIES_H_ diff --git a/localization/vision_common/package.xml b/localization/vision_common/package.xml new file mode 100644 index 0000000000..abc17e595f --- /dev/null +++ b/localization/vision_common/package.xml @@ -0,0 +1,21 @@ + + vision_common + 1.0.0 + + The vision_common package + + + Apache License, Version 2.0 + + + Astrobee Flight Software + + + Astrobee Flight Software + + catkin + localization_common + optimization_common + localization_common + optimization_common + diff --git a/localization/vision_common/readme.md b/localization/vision_common/readme.md new file mode 100644 index 0000000000..70843ee460 --- /dev/null +++ b/localization/vision_common/readme.md @@ -0,0 +1,13 @@ +\page visioncommon Vision Common + +# Package Overview + +### Distortion Models +Field of view (fov), radial (rad), and radial tangential (radtan) distortion models are provided along with an identity distortion model. These are use for example as template parameters for image projection functions in the camera utilities files. The models are templated on the value type (i.e. double, jet) to allow their usage with Ceres-based optimization packages. + +### Feature Detection And Matching +Brisk, surf, and lucas kanade optical flow feature detector and matchers are provided and inherit the base FeatureDetectorAndMatcher class so they can be used polymorphically by other classes and functions. + +### Utilities +The utilities files provide helper functions for the distortion models and templated implementations based on the distortion type of image space projections, ransac pnp, and a reprojection pose estimate function that utilities ransac pnp to find an initial pose estimate for a set of 3d and corresponding image points before performing nonliner optimization to refine this pose estimate. + diff --git a/localization/vision_common/src/brisk_feature_detector_and_matcher.cc b/localization/vision_common/src/brisk_feature_detector_and_matcher.cc new file mode 100644 index 0000000000..057dae09ab --- /dev/null +++ b/localization/vision_common/src/brisk_feature_detector_and_matcher.cc @@ -0,0 +1,48 @@ +/* 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. + */ + +#include +#include +#include + +namespace vision_common { +BriskFeatureDetectorAndMatcher::BriskFeatureDetectorAndMatcher(const BriskFeatureDetectorAndMatcherParams& params) + : params_(params), + flann_matcher_(cv::makePtr(params_.flann_table_number, params_.flann_key_size, + params_.flann_multi_probe_level)) { + detector_ = cv::BRISK::create(params_.brisk_threshold, params_.brisk_octaves, params_.brisk_float_pattern_scale); +} + +FeatureMatches BriskFeatureDetectorAndMatcher::Match(const FeatureImage& source_image, + const FeatureImage& target_image) { + std::vector matches; + flann_matcher_.match(source_image.descriptors(), target_image.descriptors(), matches); + const auto filtered_end = std::remove_if(matches.begin(), matches.end(), [this](const cv::DMatch& match) { + return match.distance > params_.max_match_hamming_distance; + }); + matches.erase(filtered_end, matches.end()); + FeatureMatches feature_matches; + for (const auto& match : matches) { + const auto& source_point = source_image.keypoints()[match.queryIdx].pt; + const auto& target_point = target_image.keypoints()[match.trainIdx].pt; + feature_matches.emplace_back(Eigen::Vector2d(source_point.x, source_point.y), + Eigen::Vector2d(target_point.x, target_point.y), match.distance); + } + return feature_matches; +} +} // namespace vision_common diff --git a/tools/calibration/src/camera_utilities.cc b/localization/vision_common/src/camera_utilities.cc similarity index 96% rename from tools/calibration/src/camera_utilities.cc rename to localization/vision_common/src/camera_utilities.cc index 108301f9d5..88ed12dc00 100644 --- a/tools/calibration/src/camera_utilities.cc +++ b/localization/vision_common/src/camera_utilities.cc @@ -16,8 +16,8 @@ * under the License. */ -#include #include +#include #include @@ -25,7 +25,7 @@ #include #include -namespace calibration { +namespace vision_common { namespace lc = localization_common; Eigen::Vector2d Project3dPointToImageSpace(const Eigen::Vector3d& cam_t_point, const Eigen::Matrix3d& intrinsics) { @@ -63,4 +63,4 @@ std::vector RandomNIndices(const int num_possible_indices, const int num_sa } return sampled_indices; } -} // namespace calibration +} // namespace vision_common diff --git a/localization/vision_common/src/good_features_to_track_detector.cc b/localization/vision_common/src/good_features_to_track_detector.cc new file mode 100644 index 0000000000..433b1b2425 --- /dev/null +++ b/localization/vision_common/src/good_features_to_track_detector.cc @@ -0,0 +1,35 @@ +/* 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. + */ + +#include + +#include +#include + +namespace cv { +GoodFeaturesToTrackDetector::GoodFeaturesToTrackDetector(const vision_common::GoodFeaturesToTrackDetectorParams& params) + : params_(params) {} +void GoodFeaturesToTrackDetector::detectAndCompute(cv::InputArray image, cv::InputArray mask, + std::vector& keypoints, cv::OutputArray descriptors, + bool useProvidedKeypoints) { + std::vector features; + cv::goodFeaturesToTrack(image, features, params_.max_corners, params_.quality_level, params_.min_distance, mask, + params_.block_size, params_.use_harris_detector, params_.k); + cv::KeyPoint::convert(features, keypoints); +} +} // namespace cv diff --git a/localization/vision_common/src/lk_optical_flow_feature_detector_and_matcher.cc b/localization/vision_common/src/lk_optical_flow_feature_detector_and_matcher.cc new file mode 100644 index 0000000000..494ef6e78f --- /dev/null +++ b/localization/vision_common/src/lk_optical_flow_feature_detector_and_matcher.cc @@ -0,0 +1,73 @@ +/* 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. + */ + +#include +#include +#include +#include + +#include +#include + +namespace vision_common { +LKOpticalFlowFeatureDetectorAndMatcher::LKOpticalFlowFeatureDetectorAndMatcher( + const LKOpticalFlowFeatureDetectorAndMatcherParams& params) + : params_(params) { + detector_.reset(new cv::GoodFeaturesToTrackDetector(params_.good_features_to_track)); +} + +FeatureMatches LKOpticalFlowFeatureDetectorAndMatcher::Match(const FeatureImage& source_image, + const FeatureImage& target_image) { + FeatureMatches matches; + if (source_image.feature_points().size() <= 0) { + LogDebug("Match: Too few feature points provided."); + return matches; + } + + const cv::TermCriteria termination_criteria(cv::TermCriteria::MAX_ITER | cv::TermCriteria::EPS, + params_.max_iterations, params_.termination_epsilon); + const cv::Size window_size(params_.window_length, params_.window_length); + // Perform forward and backward passes on detected features + std::vector matched_forward_features; + std::vector forward_status; + std::vector forward_errors; + cv::calcOpticalFlowPyrLK(source_image.image(), target_image.image(), source_image.feature_points(), + matched_forward_features, forward_status, forward_errors, window_size, params_.max_level, + termination_criteria, 0, params_.min_eigen_threshold); + std::vector matched_backward_features; + std::vector backward_status; + std::vector backward_errors; + cv::calcOpticalFlowPyrLK(target_image.image(), source_image.image(), matched_forward_features, + matched_backward_features, backward_status, backward_errors, window_size, params_.max_level, + termination_criteria, 0, params_.min_eigen_threshold); + + // Find matches between forward and backward passes + for (int i = 0; i < static_cast(matched_forward_features.size()); ++i) { + const auto& source_point = source_image.feature_points()[i]; + const auto& forward_point = matched_forward_features[i]; + if (!(forward_status[i] && backward_status[i])) continue; + const bool valid_forward_match = cv::norm(source_point - forward_point) <= params_.max_flow_distance; + const double backward_match_distance = cv::norm(source_point - matched_backward_features[i]); + const bool valid_backward_match = backward_match_distance <= params_.max_backward_match_distance; + if (!(valid_forward_match && valid_backward_match)) continue; + matches.emplace_back(Eigen::Vector2d(source_point.x, source_point.y), + Eigen::Vector2d(forward_point.x, forward_point.y), backward_match_distance); + } + return matches; +} +} // namespace vision_common diff --git a/localization/vision_common/src/parameter_reader.cc b/localization/vision_common/src/parameter_reader.cc new file mode 100644 index 0000000000..ab99b0d1f1 --- /dev/null +++ b/localization/vision_common/src/parameter_reader.cc @@ -0,0 +1,64 @@ +/* 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. + */ + +#include +#include +#include + +namespace vision_common { +namespace mc = msg_conversions; + +void LoadBriskFeatureDetectorAndMatcherParams(config_reader::ConfigReader& config, + BriskFeatureDetectorAndMatcherParams& params) { + params.brisk_threshold = mc::LoadInt(config, "brisk_threshold"); + params.brisk_octaves = mc::LoadInt(config, "brisk_octaves"); + params.brisk_float_pattern_scale = mc::LoadFloat(config, "brisk_float_pattern_scale"); + params.max_match_hamming_distance = mc::LoadInt(config, "brisk_max_match_hamming_distance"); + params.flann_table_number = mc::LoadInt(config, "brisk_flann_table_number"); + params.flann_key_size = mc::LoadInt(config, "brisk_flann_key_size"); + params.flann_multi_probe_level = mc::LoadInt(config, "brisk_flann_multi_probe_level"); +} + +void LoadGoodFeaturesToTrackDetectorParams(config_reader::ConfigReader& config, + GoodFeaturesToTrackDetectorParams& params) { + params.max_corners = mc::LoadInt(config, "lk_max_corners"); + params.quality_level = mc::LoadDouble(config, "lk_quality_level"); + params.min_distance = mc::LoadDouble(config, "lk_min_distance"); + params.block_size = mc::LoadInt(config, "lk_block_size"); + params.use_harris_detector = mc::LoadBool(config, "lk_use_harris_detector"); + params.k = mc::LoadDouble(config, "lk_k"); +} + +void LoadLKOpticalFlowFeatureDetectorAndMatcherParams(config_reader::ConfigReader& config, + LKOpticalFlowFeatureDetectorAndMatcherParams& params) { + params.max_iterations = mc::LoadInt(config, "lk_max_iterations"); + params.termination_epsilon = mc::LoadDouble(config, "lk_termination_epsilon"); + params.window_length = mc::LoadInt(config, "lk_window_length"); + params.max_level = mc::LoadInt(config, "lk_max_level"); + params.min_eigen_threshold = mc::LoadDouble(config, "lk_min_eigen_threshold"); + params.max_flow_distance = mc::LoadDouble(config, "lk_max_flow_distance"); + params.max_backward_match_distance = mc::LoadDouble(config, "lk_max_backward_match_distance"); + LoadGoodFeaturesToTrackDetectorParams(config, params.good_features_to_track); +} + +void LoadSurfFeatureDetectorAndMatcherParams(config_reader::ConfigReader& config, + SurfFeatureDetectorAndMatcherParams& params) { + params.surf_threshold = mc::LoadInt(config, "surf_threshold"); + params.max_match_distance = mc::LoadDouble(config, "surf_max_match_distance"); +} +} // namespace vision_common diff --git a/localization/vision_common/src/surf_feature_detector_and_matcher.cc b/localization/vision_common/src/surf_feature_detector_and_matcher.cc new file mode 100644 index 0000000000..a494cae43b --- /dev/null +++ b/localization/vision_common/src/surf_feature_detector_and_matcher.cc @@ -0,0 +1,47 @@ +/* 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. + */ + +#include +#include +#include + +#include + +namespace vision_common { +SurfFeatureDetectorAndMatcher::SurfFeatureDetectorAndMatcher(const SurfFeatureDetectorAndMatcherParams& params) + : params_(params) { + detector_ = cv::xfeatures2d::SURF::create(params_.surf_threshold); +} +FeatureMatches SurfFeatureDetectorAndMatcher::Match(const FeatureImage& source_image, + const FeatureImage& target_image) { + std::vector matches; + flann_matcher_.match(source_image.descriptors(), target_image.descriptors(), matches); + const auto filtered_end = std::remove_if(matches.begin(), matches.end(), [this](const cv::DMatch& match) { + return match.distance > params_.max_match_distance; + }); + matches.erase(filtered_end, matches.end()); + FeatureMatches feature_matches; + for (const auto& match : matches) { + const auto& source_point = source_image.keypoints()[match.queryIdx].pt; + const auto& target_point = target_image.keypoints()[match.trainIdx].pt; + feature_matches.emplace_back(Eigen::Vector2d(source_point.x, source_point.y), + Eigen::Vector2d(target_point.x, target_point.y), match.distance); + } + return feature_matches; +} +} // namespace vision_common diff --git a/localization/vision_common/src/test_utilities.cc b/localization/vision_common/src/test_utilities.cc new file mode 100644 index 0000000000..ea31156757 --- /dev/null +++ b/localization/vision_common/src/test_utilities.cc @@ -0,0 +1,230 @@ +/* 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. + */ + +#include +#include +#include + +#include + +#include + +namespace vision_common { +namespace lc = localization_common; + +LKOpticalFlowFeatureDetectorAndMatcherParams DefaultLKOpticalFlowFeatureDetectorAndMatcherParams() { + LKOpticalFlowFeatureDetectorAndMatcherParams params; + params.max_iterations = 10; + params.termination_epsilon = 0.03; + params.window_length = 10; + params.max_level = 3; + params.min_eigen_threshold = 0.2; + params.max_flow_distance = 50; + params.max_backward_match_distance = 0.1; + params.good_features_to_track.max_corners = 100; + params.good_features_to_track.quality_level = 0.01; + params.good_features_to_track.min_distance = 20; + params.good_features_to_track.block_size = 3; + params.good_features_to_track.use_harris_detector = false; + params.good_features_to_track.k = 0.04; + return params; +} + +cv::Mat MarkerImage(const int row_spacing, const int col_spacing, int& num_markers_added, const cv::Point2i& offset) { + cv::Mat image(cv::Mat(cv::Size(640, 480), CV_8UC1, cv::Scalar(255))); + num_markers_added = AddMarkers(row_spacing, col_spacing, image, offset); + return image; +} + +int AddMarkers(const int row_spacing, const int col_spacing, cv::Mat& image, const cv::Point2i& offset) { + int num_markers = 0; + // Don't start at zero so all markers are candidates for matches + // End before edge to add some buffer to markers + for (int row = row_spacing; row < image.rows - row_spacing; row += row_spacing) { + for (int col = col_spacing; col < image.cols - col_spacing; col += col_spacing) { + cv::drawMarker(image, offset + cv::Point2i(col, row), cv::Scalar(0), cv::MARKER_CROSS); + ++num_markers; + } + } + return num_markers; +} + +optimization_common::OptimizationParams DefaultOptimizationParams() { + optimization_common::OptimizationParams params; + params.solver_options.linear_solver_type = ceres::ITERATIVE_SCHUR; + params.solver_options.use_explicit_schur_complement = false; + params.solver_options.max_num_iterations = 100; + params.solver_options.function_tolerance = 1e-8; + params.solver_options.parameter_tolerance = 1e-8; + params.verbose = false; + params.huber_loss = 1.345; + return params; +} + +RansacPnPParams DefaultRansacPnPParams() { + RansacPnPParams params; + params.max_inlier_threshold = 3; + params.num_iterations = 100; + params.min_num_inliers = 4; + // TODO(rsoussan): Change this to cv::SOLVEPNP_IPPE when available. + // Currenty cv::SOLVEPNP_P3P and cv::SOLVEPNP_ITERATIVE lead to significant errors even with perfect data + params.pnp_method = cv::SOLVEPNP_EPNP; + return params; +} + +ReprojectionPoseEstimateParams DefaultReprojectionPoseEstimateParams() { + ReprojectionPoseEstimateParams params; + params.optimization = DefaultOptimizationParams(); + params.ransac_pnp = DefaultRansacPnPParams(); + params.optimize_estimate = true; + params.max_inlier_threshold = 3.0; + return params; +} + +Eigen::VectorXd RandomFovDistortion() { + Eigen::VectorXd distortion(1); + distortion[0] = lc::RandomDouble(0.1, 0.3); + return distortion; +} + +Eigen::VectorXd RandomRadDistortion() { + Eigen::VectorXd distortion(2); + distortion[0] = lc::RandomDouble(-0.1, 0.1); + distortion[1] = lc::RandomDouble(-0.1, 0.1); + return distortion; +} + +Eigen::VectorXd RandomRadTanDistortion() { + Eigen::VectorXd distortion(4); + distortion[0] = lc::RandomDouble(-0.1, 0.1); + distortion[1] = lc::RandomDouble(-0.1, 0.1); + distortion[2] = lc::RandomDouble(0, 0.1); + distortion[3] = lc::RandomDouble(0, 0.1); + return distortion; +} + +std::vector TargetPoints(const int points_per_row, const int points_per_col, const double row_spacing, + const double col_spacing) { + Eigen::Vector3d target_center(points_per_col * col_spacing / 2.0, points_per_row * row_spacing / 2.0, 0.0); + std::vector target_points; + for (int i = 0; i < points_per_col; ++i) { + for (int j = 0; j < points_per_row; ++j) { + // Center target points about (0,0) + target_points.emplace_back(Eigen::Vector3d(i * col_spacing, j * row_spacing, 0) - target_center); + } + } + return target_points; +} + +std::vector RandomFrontFacingPoints(const int num_points) { + std::vector points; + for (int i = 0; i < num_points; ++i) { + points.emplace_back(RandomFrontFacingPoint()); + } + return points; +} + +Eigen::Vector3d RandomFrontFacingPoint() { + static constexpr double rho_min = 1.0; + static constexpr double rho_max = 3.0; + static constexpr double phi_min = -25.0; + static constexpr double phi_max = 25.0; + static constexpr double z_rho_scale = 0.5; + return RandomFrontFacingPoint(rho_min, rho_max, phi_min, phi_max, z_rho_scale); +} + +Eigen::Vector3d RandomFrontFacingPoint(const double rho_min, const double rho_max, const double phi_min, + const double phi_max, const double z_rho_scale) { + const double rho = lc::RandomDouble(rho_min, rho_max); + const double phi = lc::RandomDouble(phi_min, phi_max); + const double z = lc::RandomDouble(-1.0 * z_rho_scale * rho, z_rho_scale * rho); + // Z and x are swapped so z defines distance from camera rather than height + const Eigen::Vector3d tmp = lc::CylindricalToCartesian(Eigen::Vector3d(rho, phi, z)); + const Eigen::Vector3d random_point(tmp.z(), tmp.y(), tmp.x()); + return random_point; +} + +Eigen::Isometry3d RandomFrontFacingPose() { + static constexpr double rho_min = 1.0; + static constexpr double rho_max = 3.0; + static constexpr double phi_min = -25.0; + static constexpr double phi_max = 25.0; + static constexpr double z_rho_scale = 0.5; + + // Pitch acts like yaw since z axis points outwards in camera frame + static constexpr double yaw_min = -10.0; + static constexpr double yaw_max = 10.0; + static constexpr double pitch_min = -45; + static constexpr double pitch_max = 45; + static constexpr double roll_min = -10; + static constexpr double roll_max = 10; + + return RandomFrontFacingPose(rho_min, rho_max, phi_min, phi_max, z_rho_scale, yaw_min, yaw_max, pitch_min, pitch_max, + roll_min, roll_max); +} + +Eigen::Isometry3d RandomFrontFacingPose(const double rho_min, const double rho_max, const double phi_min, + const double phi_max, const double z_rho_scale, const double yaw_min, + const double yaw_max, const double pitch_min, const double pitch_max, + const double roll_min, const double roll_max) { + const Eigen::Vector3d translation = RandomFrontFacingPoint(rho_min, rho_max, phi_min, phi_max, z_rho_scale); + const double yaw = lc::RandomDouble(yaw_min, yaw_max); + const double pitch = lc::RandomDouble(pitch_min, pitch_max); + const double roll = lc::RandomDouble(roll_min, roll_max); + const Eigen::Matrix3d rotation = lc::RotationFromEulerAngles(yaw, pitch, roll); + return lc::Isometry3d(translation, rotation); +} + +std::vector EvenlySpacedTargetPoses(const int num_rows, const int num_cols, const int num_y_levels) { + // Pitch acts like yaw since z axis points out of camera frame + const lc::Sampler pitch_sampler(-15.0, 15.0, num_cols); + const lc::Sampler roll_sampler(-15.0, 15.0, num_cols); + const lc::Sampler yaw_sampler(-15.0, 15.0, num_cols); + + // Cylindrical coordinates for translation + const lc::Sampler rho_sampler(1.0, 3.0, num_rows); + constexpr double phi = 0.0; + + // Use smaller y_rho scale factor since image is shorter than it is wide + constexpr double y_rho_scale = 0.235; + constexpr double z_rho_scale = 0.5; + + std::vector poses; + for (int i = 0; i < num_rows; ++i) { + const double rho = rho_sampler.Sample(i); + const lc::Sampler z_sampler(-1.0 * rho * z_rho_scale, rho * z_rho_scale, num_cols); + for (int j = 0; j < num_cols; ++j) { + const double pitch = pitch_sampler.Sample(j); + const double roll = roll_sampler.Sample(j); + const double yaw = yaw_sampler.Sample(j); + const double z = z_sampler.Sample(j); + // Z and x are swapped so z defines distance from camera rather than height + const Eigen::Vector3d tmp = lc::CylindricalToCartesian(Eigen::Vector3d(rho, phi, z)); + const Eigen::Matrix3d rotation = lc::RotationFromEulerAngles(yaw, pitch, roll); + const lc::Sampler y_sampler(-1.0 * rho * y_rho_scale, rho * y_rho_scale, num_y_levels); + for (int k = 0; k < num_y_levels; ++k) { + const double y = y_sampler.Sample(k); + const Eigen::Vector3d translation(tmp.z(), y, tmp.x()); + poses.emplace_back(lc::Isometry3d(translation, rotation)); + } + } + } + + return poses; +} +} // namespace vision_common diff --git a/tools/calibration/test/test_camera_utilities.cc b/localization/vision_common/test/test_camera_utilities.cc similarity index 84% rename from tools/calibration/test/test_camera_utilities.cc rename to localization/vision_common/test/test_camera_utilities.cc index 11b7030dcc..c8a8f91b08 100644 --- a/tools/calibration/test/test_camera_utilities.cc +++ b/localization/vision_common/test/test_camera_utilities.cc @@ -16,26 +16,24 @@ * under the License. */ -#include "test_utilities.h" // NOLINT - -#include #include #include -#include -#include +#include +#include +#include #include -namespace ca = calibration; namespace lc = localization_common; namespace oc = optimization_common; +namespace vc = vision_common; TEST(CameraUtilitiesTester, Inliers) { - const auto params = ca::DefaultReprojectionPoseEstimateParams(); + const auto params = vc::DefaultReprojectionPoseEstimateParams(); const double inlier_threshold = 3.0; const int num_desired_points = 20; for (int i = 0; i < 500; ++i) { - const auto correspondences = ca::RegistrationCorrespondences( - ca::RandomFrontFacingPose(), lc::RandomIntrinsics(), ca::RandomFrontFacingPoints(num_desired_points)); + const auto correspondences = vc::RegistrationCorrespondences( + vc::RandomFrontFacingPose(), lc::RandomIntrinsics(), vc::RandomFrontFacingPoints(num_desired_points)); const int num_points = static_cast(correspondences.correspondences().size()); std::vector noisy_image_points; std::unordered_set noisy_point_indices; @@ -55,7 +53,7 @@ TEST(CameraUtilitiesTester, Inliers) { } } std::vector inliers; - const int num_inliers = ca::Inliers( + const int num_inliers = vc::Inliers( noisy_image_points, correspondences.correspondences().points_3d, correspondences.intrinsics(), Eigen::VectorXd(1), correspondences.camera_T_target(), inlier_threshold, inliers); ASSERT_EQ(num_points, inlier_point_indices.size() + noisy_point_indices.size()); diff --git a/tools/calibration/test/test_camera_utilities.test b/localization/vision_common/test/test_camera_utilities.test similarity index 93% rename from tools/calibration/test/test_camera_utilities.test rename to localization/vision_common/test/test_camera_utilities.test index 70780045b8..bee785cf41 100644 --- a/tools/calibration/test/test_camera_utilities.test +++ b/localization/vision_common/test/test_camera_utilities.test @@ -16,5 +16,5 @@ - + diff --git a/localization/vision_common/test/test_lk_optical_flow_feature_detector_and_matcher.cc b/localization/vision_common/test/test_lk_optical_flow_feature_detector_and_matcher.cc new file mode 100644 index 0000000000..dc762fa88f --- /dev/null +++ b/localization/vision_common/test/test_lk_optical_flow_feature_detector_and_matcher.cc @@ -0,0 +1,62 @@ +/* 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. + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace lc = localization_common; +namespace vc = vision_common; + +TEST(FeatureDetectorAndMatcherTester, LKOpticalFlow) { + auto params = vc::DefaultLKOpticalFlowFeatureDetectorAndMatcherParams(); + params.good_features_to_track.max_corners = 10000000; + vc::LKOpticalFlowFeatureDetectorAndMatcher lk_detector_and_matcher(params); + const int row_spacing = 33; + const int col_spacing = 33; + int num_markers_added; + const cv::Point2i offset(5, 5); + const auto image_a = vc::MarkerImage(row_spacing, col_spacing, num_markers_added); + const auto image_b = vc::MarkerImage(row_spacing, col_spacing, num_markers_added, offset); + vc::FeatureImage feature_image_a(image_a, *(lk_detector_and_matcher.detector())); + vc::FeatureImage feature_image_b(image_b, *(lk_detector_and_matcher.detector())); + const auto matches = lk_detector_and_matcher.Match(feature_image_a, feature_image_b); + EXPECT_EQ(matches.size(), num_markers_added); + for (const auto& match : matches) { + const Eigen::Vector2d match_offset = match.target_point - match.source_point; + EXPECT_NEAR(match_offset.x(), offset.x, 1e-1); + EXPECT_NEAR(match_offset.y(), offset.y, 1e-1); + } +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/vision_common/test/test_lk_optical_flow_feature_detector_and_matcher.cc_old_with_images b/localization/vision_common/test/test_lk_optical_flow_feature_detector_and_matcher.cc_old_with_images new file mode 100644 index 0000000000..172673b97f --- /dev/null +++ b/localization/vision_common/test/test_lk_optical_flow_feature_detector_and_matcher.cc_old_with_images @@ -0,0 +1,75 @@ +/* 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. + */ + +#include "test_utilities.h" // NOLINT +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace lc = localization_common; +namespace vc = vision_common; + +TEST(FeatureDetectorAndMatcherTester, LKOpticalFlow) { + auto params = vc::DefaultLKOpticalFlowFeatureDetectorAndMatcherParams(); + params.good_features_to_track.max_corners = 10000000; + vc::LKOpticalFlowFeatureDetectorAndMatcher lk_detector_and_matcher(params); + const int row_spacing = 33; + const int col_spacing = 33; + cv::Mat image_a(cv::Mat(cv::Size(640, 480), CV_8UC1, cv::Scalar(255))); + const int num_markers = vc::AddMarkers(row_spacing, col_spacing, image_a); + cv::Mat image_b(cv::Mat(cv::Size(640, 480), CV_8UC1, cv::Scalar(255))); + const cv::Point2i offset(5, 5); + vc::AddMarkers(row_spacing, col_spacing, image_b, offset); + vc::FeatureImage feature_image_a(image_a, *(lk_detector_and_matcher.detector())); + vc::FeatureImage feature_image_b(image_b, *(lk_detector_and_matcher.detector())); + for (const auto& p : feature_image_a.feature_points()) { + cv::drawMarker(image_a, p, cv::Scalar(128), cv::MARKER_DIAMOND); + } + for (const auto& p : feature_image_b.feature_points()) { + cv::drawMarker(image_b, p, cv::Scalar(128), cv::MARKER_DIAMOND); + } + const auto matches = lk_detector_and_matcher.Match(feature_image_a, feature_image_b); + EXPECT_EQ(matches.size(), num_markers); + for (const auto& match : matches) { + const Eigen::Vector2d match_offset = match.target_point - match.source_point; + + cv::drawMarker(image_a, cv::Point2i(std::round(match.source_point.x()), std::round(match.source_point.y())), cv::Scalar(128), cv::MARKER_STAR); + cv::drawMarker(image_b, cv::Point2i(std::round(match.target_point.x()), std::round(match.target_point.y())), cv::Scalar(128), cv::MARKER_STAR); + EXPECT_NEAR(match_offset.x(), offset.x, 1e-1); + EXPECT_NEAR(match_offset.y(), offset.y, 1e-1); + } + cv::imshow("image a", image_a); + cv::imshow("image b", image_b); + cv::waitKey(0); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/localization/vision_common/test/test_lk_optical_flow_feature_detector_and_matcher.test b/localization/vision_common/test/test_lk_optical_flow_feature_detector_and_matcher.test new file mode 100644 index 0000000000..e912cf200c --- /dev/null +++ b/localization/vision_common/test/test_lk_optical_flow_feature_detector_and_matcher.test @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/tools/calibration/test/test_pnp.cc b/localization/vision_common/test/test_pnp.cc similarity index 84% rename from tools/calibration/test/test_pnp.cc rename to localization/vision_common/test/test_pnp.cc index 7dd51ba588..330dd8f1e0 100644 --- a/tools/calibration/test/test_pnp.cc +++ b/localization/vision_common/test/test_pnp.cc @@ -16,21 +16,18 @@ * under the License. */ -#include "test_utilities.h" // NOLINT - -#include #include #include -#include -#include +#include +#include +#include #include #include -namespace ca = calibration; namespace lc = localization_common; -namespace oc = optimization_common; +namespace vc = vision_common; // TODO(rsoussan): Put back ransacpnp tests once pnp issues are resolved /*TEST(RansacPnPTester, RansacPnP) { Eigen::Isometry3d pose = Eigen::Isometry3d::Identity(); @@ -41,7 +38,7 @@ namespace oc = optimization_common; intrinsics(0, 2) = 500; intrinsics(1, 2) = 500; for (int i = 0; i < 500; ++i) { - const auto correspondences = ca::RegistrationCorrespondences(pose, intrinsics, ca::TargetPoints(2, 2)); + const auto correspondences = vc::RegistrationCorrespondences(pose, intrinsics, vc::TargetPoints(2, 2)); std::vector image_points_cv; for (const auto& image_point : correspondences.correspondences().image_points) { image_points_cv.emplace_back(cv::Point2d(image_point.x(), image_point.y())); @@ -56,9 +53,9 @@ namespace oc = optimization_common; cv::eigen2cv(correspondences.intrinsics(), intrinsics_cv); cv::Mat rodrigues_rotation_cv(3, 1, cv::DataType::type, cv::Scalar(0)); cv::Mat translation_cv(3, 1, cv::DataType::type, cv::Scalar(0)); - ca::UndistortedPnP(image_points_cv, points_3d_cv, intrinsics_cv, cv::SOLVEPNP_ITERATIVE, rodrigues_rotation_cv, + vc::UndistortedPnP(image_points_cv, points_3d_cv, intrinsics_cv, cv::SOLVEPNP_ITERATIVE, rodrigues_rotation_cv, translation_cv); - const Eigen::Isometry3d pose_estimate = ca::Isometry3d(rodrigues_rotation_cv, translation_cv); + const Eigen::Isometry3d pose_estimate = vc::Isometry3d(rodrigues_rotation_cv, translation_cv); LogError("true pose: " << std::endl << correspondences.camera_T_target().matrix()); LogError("pnp pose: " << std::endl << pose_estimate.matrix()); // TODO(rsoussan): Decrease tolerance once cv::solvePnP issues are resolved diff --git a/tools/calibration/test/test_pnp.test b/localization/vision_common/test/test_pnp.test similarity index 95% rename from tools/calibration/test/test_pnp.test rename to localization/vision_common/test/test_pnp.test index cc82d9b80d..b6da406e03 100644 --- a/tools/calibration/test/test_pnp.test +++ b/localization/vision_common/test/test_pnp.test @@ -16,5 +16,5 @@ - + diff --git a/tools/calibration/test/test_ransac_pnp.cc b/localization/vision_common/test/test_ransac_pnp.cc similarity index 79% rename from tools/calibration/test/test_ransac_pnp.cc rename to localization/vision_common/test/test_ransac_pnp.cc index cd8e3638c7..520712ba4b 100644 --- a/tools/calibration/test/test_ransac_pnp.cc +++ b/localization/vision_common/test/test_ransac_pnp.cc @@ -16,37 +16,34 @@ * under the License. */ -#include "test_utilities.h" // NOLINT - -#include #include #include -#include -#include +#include +#include +#include #include #include -namespace ca = calibration; namespace lc = localization_common; -namespace oc = optimization_common; +namespace vc = vision_common; // TODO(rsoussan): Put back RansacPnP tests once pnp issues are resolved /*TEST(RansacPnPTester, EvenlySpacedTargetsIdentityDistortionWithNoise) { - const auto params = ca::DefaultRansacPnPParams(); + const auto params = vc::DefaultRansacPnPParams(); const int num_rows = 5; const int num_cols = 5; const int num_y_levels = 5; - const auto target_poses = ca::EvenlySpacedTargetPoses(num_rows, num_cols, num_y_levels); - const auto target_points = ca::TargetPoints(10, 10); + const auto target_poses = vc::EvenlySpacedTargetPoses(num_rows, num_cols, num_y_levels); + const auto target_points = vc::TargetPoints(10, 10); std::vector initial_inliers(target_points.size()); // Fill inliers with all indices std::iota(initial_inliers.begin(), initial_inliers.end(), 0); for (int i = 0; i < target_poses.size(); ++i) { const auto correspondences = - ca::RegistrationCorrespondences(target_poses[i], lc::RandomIntrinsics(), target_points); - const auto pose_estimate = ca::RansacPnP( + vc::RegistrationCorrespondences(target_poses[i], lc::RandomIntrinsics(), target_points); + const auto pose_estimate = vc::RansacPnP( correspondences.correspondences().image_points, correspondences.correspondences().points_3d, correspondences.intrinsics(), Eigen::VectorXd(), params); ASSERT_TRUE(pose_estimate != boost::none); diff --git a/tools/calibration/test/test_ransac_pnp.test b/localization/vision_common/test/test_ransac_pnp.test similarity index 94% rename from tools/calibration/test/test_ransac_pnp.test rename to localization/vision_common/test/test_ransac_pnp.test index 859d4e2f11..b6b8811fce 100644 --- a/tools/calibration/test/test_ransac_pnp.test +++ b/localization/vision_common/test/test_ransac_pnp.test @@ -16,5 +16,5 @@ - + diff --git a/tools/calibration/test/test_reprojection_pose_estimate.cc b/localization/vision_common/test/test_reprojection_pose_estimate.cc similarity index 75% rename from tools/calibration/test/test_reprojection_pose_estimate.cc rename to localization/vision_common/test/test_reprojection_pose_estimate.cc index 424b11e307..27c83996e0 100644 --- a/tools/calibration/test/test_reprojection_pose_estimate.cc +++ b/localization/vision_common/test/test_reprojection_pose_estimate.cc @@ -16,29 +16,26 @@ * under the License. */ -#include "test_utilities.h" // NOLINT - -#include #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include -namespace ca = calibration; namespace lc = localization_common; -namespace oc = optimization_common; +namespace vc = vision_common; TEST(ReprojectionPoseEstimateTester, EvenlySpacedTargetsIdentityDistortionWithNoise) { - const auto params = ca::DefaultReprojectionPoseEstimateParams(); + const auto params = vc::DefaultReprojectionPoseEstimateParams(); const int num_rows = 5; const int num_cols = 5; const int num_y_levels = 5; - const auto target_poses = ca::EvenlySpacedTargetPoses(num_rows, num_cols, num_y_levels); - const auto target_points = ca::TargetPoints(10, 10); + const auto target_poses = vc::EvenlySpacedTargetPoses(num_rows, num_cols, num_y_levels); + const auto target_points = vc::TargetPoints(10, 10); std::vector initial_inliers(target_points.size()); // Fill inliers with all indices std::iota(initial_inliers.begin(), initial_inliers.end(), 0); @@ -46,10 +43,10 @@ TEST(ReprojectionPoseEstimateTester, EvenlySpacedTargetsIdentityDistortionWithNo const double initial_estimate_rotation_noise = 0.1; for (int i = 0; i < static_cast(target_poses.size()); ++i) { const auto correspondences = - ca::RegistrationCorrespondences(target_poses[i], lc::RandomIntrinsics(), target_points); + vc::RegistrationCorrespondences(target_poses[i], lc::RandomIntrinsics(), target_points); const Eigen::Isometry3d noisy_initial_estimate = lc::AddNoiseToIsometry3d( correspondences.camera_T_target(), initial_estimate_translation_noise, initial_estimate_rotation_noise); - const auto pose_estimate = ca::ReprojectionPoseEstimateWithInitialEstimate( + const auto pose_estimate = vc::ReprojectionPoseEstimateWithInitialEstimate( correspondences.correspondences().image_points, correspondences.correspondences().points_3d, correspondences.intrinsics(), Eigen::VectorXd(1), params, noisy_initial_estimate, initial_inliers); ASSERT_TRUE(pose_estimate != boost::none); @@ -59,24 +56,24 @@ TEST(ReprojectionPoseEstimateTester, EvenlySpacedTargetsIdentityDistortionWithNo } TEST(ReprojectionPoseEstimateTester, EvenlySpacedTargetsFovDistortionWithNoise) { - const auto params = ca::DefaultReprojectionPoseEstimateParams(); + const auto params = vc::DefaultReprojectionPoseEstimateParams(); const int num_rows = 5; const int num_cols = 5; const int num_y_levels = 5; - const auto target_poses = ca::EvenlySpacedTargetPoses(num_rows, num_cols, num_y_levels); - const auto target_points = ca::TargetPoints(10, 10); + const auto target_poses = vc::EvenlySpacedTargetPoses(num_rows, num_cols, num_y_levels); + const auto target_points = vc::TargetPoints(10, 10); std::vector initial_inliers(target_points.size()); // Fill inliers with all indices std::iota(initial_inliers.begin(), initial_inliers.end(), 0); const double initial_estimate_translation_noise = 0.1; const double initial_estimate_rotation_noise = 0.1; for (int i = 0; i < static_cast(target_poses.size()); ++i) { - const auto distortion = ca::RandomFovDistortion(); - const auto correspondences = ca::RegistrationCorrespondences( + const auto distortion = vc::RandomFovDistortion(); + const auto correspondences = vc::RegistrationCorrespondences( target_poses[i], lc::RandomIntrinsics(), target_points, distortion); const Eigen::Isometry3d noisy_initial_estimate = lc::AddNoiseToIsometry3d( correspondences.camera_T_target(), initial_estimate_translation_noise, initial_estimate_rotation_noise); - const auto pose_estimate = ca::ReprojectionPoseEstimateWithInitialEstimate( + const auto pose_estimate = vc::ReprojectionPoseEstimateWithInitialEstimate( correspondences.correspondences().image_points, correspondences.correspondences().points_3d, correspondences.intrinsics(), distortion, params, noisy_initial_estimate, initial_inliers); ASSERT_TRUE(pose_estimate != boost::none); @@ -86,24 +83,24 @@ TEST(ReprojectionPoseEstimateTester, EvenlySpacedTargetsFovDistortionWithNoise) } TEST(ReprojectionPoseEstimateTester, EvenlySpacedTargetsRadDistortionWithNoise) { - const auto params = ca::DefaultReprojectionPoseEstimateParams(); + const auto params = vc::DefaultReprojectionPoseEstimateParams(); const int num_rows = 5; const int num_cols = 5; const int num_y_levels = 5; - const auto target_poses = ca::EvenlySpacedTargetPoses(num_rows, num_cols, num_y_levels); - const auto target_points = ca::TargetPoints(10, 10); + const auto target_poses = vc::EvenlySpacedTargetPoses(num_rows, num_cols, num_y_levels); + const auto target_points = vc::TargetPoints(10, 10); std::vector initial_inliers(target_points.size()); // Fill inliers with all indices std::iota(initial_inliers.begin(), initial_inliers.end(), 0); const double initial_estimate_translation_noise = 0.1; const double initial_estimate_rotation_noise = 0.1; for (int i = 0; i < static_cast(target_poses.size()); ++i) { - const auto distortion = ca::RandomRadDistortion(); - const auto correspondences = ca::RegistrationCorrespondences( + const auto distortion = vc::RandomRadDistortion(); + const auto correspondences = vc::RegistrationCorrespondences( target_poses[i], lc::RandomIntrinsics(), target_points, distortion); const Eigen::Isometry3d noisy_initial_estimate = lc::AddNoiseToIsometry3d( correspondences.camera_T_target(), initial_estimate_translation_noise, initial_estimate_rotation_noise); - const auto pose_estimate = ca::ReprojectionPoseEstimateWithInitialEstimate( + const auto pose_estimate = vc::ReprojectionPoseEstimateWithInitialEstimate( correspondences.correspondences().image_points, correspondences.correspondences().points_3d, correspondences.intrinsics(), distortion, params, noisy_initial_estimate, initial_inliers); ASSERT_TRUE(pose_estimate != boost::none); @@ -113,24 +110,24 @@ TEST(ReprojectionPoseEstimateTester, EvenlySpacedTargetsRadDistortionWithNoise) } TEST(ReprojectionPoseEstimateTester, EvenlySpacedTargetsRadTanDistortionWithNoise) { - const auto params = ca::DefaultReprojectionPoseEstimateParams(); + const auto params = vc::DefaultReprojectionPoseEstimateParams(); const int num_rows = 5; const int num_cols = 5; const int num_y_levels = 5; - const auto target_poses = ca::EvenlySpacedTargetPoses(num_rows, num_cols, num_y_levels); - const auto target_points = ca::TargetPoints(10, 10); + const auto target_poses = vc::EvenlySpacedTargetPoses(num_rows, num_cols, num_y_levels); + const auto target_points = vc::TargetPoints(10, 10); std::vector initial_inliers(target_points.size()); // Fill inliers with all indices std::iota(initial_inliers.begin(), initial_inliers.end(), 0); const double initial_estimate_translation_noise = 0.1; const double initial_estimate_rotation_noise = 0.1; for (int i = 0; i < static_cast(target_poses.size()); ++i) { - const auto distortion = ca::RandomRadTanDistortion(); - const auto correspondences = ca::RegistrationCorrespondences( + const auto distortion = vc::RandomRadTanDistortion(); + const auto correspondences = vc::RegistrationCorrespondences( target_poses[i], lc::RandomIntrinsics(), target_points, distortion); const Eigen::Isometry3d noisy_initial_estimate = lc::AddNoiseToIsometry3d( correspondences.camera_T_target(), initial_estimate_translation_noise, initial_estimate_rotation_noise); - const auto pose_estimate = ca::ReprojectionPoseEstimateWithInitialEstimate( + const auto pose_estimate = vc::ReprojectionPoseEstimateWithInitialEstimate( correspondences.correspondences().image_points, correspondences.correspondences().points_3d, correspondences.intrinsics(), distortion, params, noisy_initial_estimate, initial_inliers); ASSERT_TRUE(pose_estimate != boost::none); diff --git a/tools/calibration/test/test_reprojection_pose_estimate.test b/localization/vision_common/test/test_reprojection_pose_estimate.test similarity index 92% rename from tools/calibration/test/test_reprojection_pose_estimate.test rename to localization/vision_common/test/test_reprojection_pose_estimate.test index a20fc646e9..cce80a049f 100644 --- a/tools/calibration/test/test_reprojection_pose_estimate.test +++ b/localization/vision_common/test/test_reprojection_pose_estimate.test @@ -16,5 +16,5 @@ - + diff --git a/management/cpu_mem_monitor/include/cpu_mem_monitor/cpu_mem_monitor.h b/management/cpu_mem_monitor/include/cpu_mem_monitor/cpu_mem_monitor.h index baf94a6810..52631b9630 100644 --- a/management/cpu_mem_monitor/include/cpu_mem_monitor/cpu_mem_monitor.h +++ b/management/cpu_mem_monitor/include/cpu_mem_monitor/cpu_mem_monitor.h @@ -74,7 +74,7 @@ class CpuMemMonitor : public ff_util::FreeFlyerNodelet { private: // Get the PIDs of the nodes to monitor - int GetPIDs(); + void GetPIDs(ros::TimerEvent const &te); // Assert CPU loads and report if too high void AssertCPULoadHighFaultCallback(ros::TimerEvent const& te); @@ -123,20 +123,23 @@ class CpuMemMonitor : public ff_util::FreeFlyerNodelet { ros::Publisher cpu_state_pub_; // Cpu stats publisher ros::Publisher mem_state_pub_; // Memory stats publisher ros::Timer reload_params_timer_; // Ckeck if parameters were updated + ros::Timer pid_timer_; // Update PIDs ros::Timer stats_timer_; // Update stats ros::Timer assert_cpu_load_fault_timer_; // Check cpu load limits ros::Timer clear_cpu_load_fault_timer_; // Clear cpu fault timer ros::Timer assert_mem_load_fault_timer_; // Check memory load limits ros::Timer clear_mem_load_fault_timer_; // Clear memory fault timer - int pub_queue_size_, update_freq_hz_; // Monitor publishing queue size and frequency + int pub_queue_size_; // Monitor publishing queue size + double update_freq_hz_, update_pid_hz_; // Publishing and PID update frequency struct sysinfo mem_info_; // Scope memory info from sysinfo unsigned int ncpus_; // Number of cpu's std::string processor_name_; // Processor running this monitor (mlp,llp) - // Store PID values std::string monitor_host_; // Either mlp, llp, hlp - std::map nodes_pid_; + + std::map nodes_pid_; // Map PID values + std::map nodes_proc_time_; // Map proc time values // CPU Monitor Parameters bool temp_fault_triggered_; // Temperature fault triggered diff --git a/management/cpu_mem_monitor/src/cpu_mem_monitor.cc b/management/cpu_mem_monitor/src/cpu_mem_monitor.cc index d4b9ca8bfb..b8ea750a05 100644 --- a/management/cpu_mem_monitor/src/cpu_mem_monitor.cc +++ b/management/cpu_mem_monitor/src/cpu_mem_monitor.cc @@ -78,7 +78,7 @@ void CpuMemMonitor::Initialize(ros::NodeHandle *nh) { true, false); - // Timer for clearing the cpu load too high fault + // Timer for clearing the memory load too high fault clear_mem_load_fault_timer_ = nh->createTimer( ros::Duration(clear_load_high_fault_timeout_sec_), &CpuMemMonitor::ClearMemLoadHighFaultCallback, @@ -86,8 +86,15 @@ void CpuMemMonitor::Initialize(ros::NodeHandle *nh) { true, false); - // Timer for checking cpu stats. Timer is not one shot and start it right away - stats_timer_ = nh->createTimer(ros::Duration(update_freq_hz_), + // Timer for updating PID values of interest nodes + pid_timer_ = nh->createTimer(ros::Duration(1 / update_pid_hz_), + &CpuMemMonitor::GetPIDs, + this, + false, + true); + + // Timer for checking cpu and memory stats. Timer is not one shot and start it right away + stats_timer_ = nh->createTimer(ros::Duration(1 / update_freq_hz_), &CpuMemMonitor::PublishStatsCallback, this, false, @@ -101,7 +108,7 @@ void CpuMemMonitor::Initialize(ros::NodeHandle *nh) { ros::master::execute("lookupNode", args, result, payload, true); monitor_host_ = getHostfromURI(result[2]); if (monitor_host_.empty()) { - ROS_ERROR_STREAM("URI of the memory monitor not valid"); + FF_ERROR("URI of the memory monitor not valid"); return; } @@ -145,6 +152,7 @@ void CpuMemMonitor::Initialize(ros::NodeHandle *nh) { // Five load fields: nice, user, sys, virt, total cpu_state_msg_.cpus[i].loads.resize(5); } + cpu_state_msg_.load_nodes.resize(nodes_pid_.size()); // Initialize the memory state message mem_state_msg_.name = monitor_host_; @@ -165,8 +173,17 @@ bool CpuMemMonitor::ReadParams() { config_reader::ConfigReader::Table processor_config(&config_params_, processor_name_.c_str()); + // get udpate pid frequency + if (!processor_config.GetPosReal("update_pid_hz", &update_pid_hz_)) { + err_msg = "CPU monitor: Update PID frequency not specified for " + + processor_name_; + FF_ERROR(err_msg); + this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg); + return false; + } + // get udpate stats frequency - if (!processor_config.GetInt("update_freq_hz", &update_freq_hz_)) { + if (!processor_config.GetPosReal("update_freq_hz", &update_freq_hz_)) { err_msg = "CPU monitor: Update frequency not specified for " + processor_name_; FF_ERROR(err_msg); @@ -243,12 +260,12 @@ bool CpuMemMonitor::ReadParams() { for (int i = 0; i < nodes.GetSize(); i++) { config_reader::ConfigReader::Table node; if (!nodes.GetTable(i + 1, &node)) { - ROS_ERROR("Could not get node table"); + FF_ERROR("Could not get node table"); return false; } std::string name; if (node.GetStr("name", &name)) { - ROS_ERROR_STREAM("Read node " << name); + NODELET_DEBUG_STREAM("Read node " << name); nodes_pid_.insert(std::pair(name, 0)); } } @@ -293,7 +310,7 @@ void CpuMemMonitor::ClearMemLoadHighFaultCallback(ros::TimerEvent const& te) { load_fault_state_ = CLEARED; } -int CpuMemMonitor::GetPIDs() { +void CpuMemMonitor::GetPIDs(ros::TimerEvent const &te) { // Go through all the node list and get the PID XmlRpc::XmlRpcValue args, result, payload; std::map::iterator it; @@ -321,7 +338,7 @@ int CpuMemMonitor::GetPIDs() { it->second = -1; std::string err_msg = "CPU Memory Monitor: Specified node " + it->first + "in" + monitor_host_ + " and not in the same cpu as manager " + monitor_host_ + "."; - FF_WARN(err_msg); + FF_ERROR(err_msg); continue; } @@ -333,7 +350,7 @@ int CpuMemMonitor::GetPIDs() { if (!pipe) { it->second = -1; std::string err_msg = "CPU Memory Monitor: Could not open rosnode process for node " + it->first; - FF_WARN(err_msg); + FF_ERROR(err_msg); continue; } while (fgets(buffer.data(), 128, pipe) != NULL) { @@ -345,7 +362,7 @@ int CpuMemMonitor::GetPIDs() { it->second = -1; std::string err_msg = "CPU Memory Monitor: Specified node " + it->first + "does not have a PID."; - FF_WARN(err_msg); + FF_ERROR(err_msg); continue; } pclose(pipe); @@ -353,7 +370,7 @@ int CpuMemMonitor::GetPIDs() { it->second = std::stoi(pid); } } - return 0; + return; } int CpuMemMonitor::CollectCPUStats() { @@ -362,6 +379,7 @@ int CpuMemMonitor::CollectCPUStats() { total, idle_all, system_all, virtual_all = 0; uint64_t total_period, user_period, nice_period, steal_period, guest_period, system_all_period; + uint64_t total_period_all; uint32_t cpuid = 0; char buffer[256]; @@ -428,6 +446,7 @@ int CpuMemMonitor::CollectCPUStats() { total_period = 1; double totald = static_cast(total_period); + if (i == 0) total_period_all = totald; load_cpus_[i].nice_percentage = nice_period / totald * 100.0d; load_cpus_[i].user_percentage = user_period / totald * 100.0d; load_cpus_[i].system_percentage = system_all_period / totald * 100.0d; @@ -461,6 +480,54 @@ int CpuMemMonitor::CollectCPUStats() { cpu_state_msg_.cpus[i].loads[4] = load_cpus_[(i + 1)].total_percentage; } + // Go through all the node list and calculate cpu usage + uint64_t utime, stime; + double cpu_usage_node; + std::map::iterator it; + std::map::iterator it_load; + int i; + for ( i = 0, it = nodes_pid_.begin(); it != nodes_pid_.end(); i++, it++ ) { + // Look if PID is invalid + if (it->second <= 0) + continue; + + // Get CPU useage for individual nodes + cpu_state_msg_.load_nodes[i].name = it->first; + FILE* file = fopen(("/proc/" + std::to_string(it->second) + "/stat").c_str(), "r"); + if (!file) { + continue; + } + char buffer[256]; + if (fgets(buffer, sizeof(buffer), file) == NULL) { + fprintf(stderr, "fgets screwed up\n"); + break; + } + + if (sscanf(buffer, "%*d %*s %*c %*d" // pid, command, state, ppid + "%*d %*d %*d %*d %*u %*u %*u %*u %*u" + "%" SCNu64 " %" SCNu64 // usertime, systemtime + "%*d %*d %*d %*d %*d %*d %*u" + "%*u", // virtual memory size in bytes + &utime, &stime) != 2) + continue; + fclose(file); + + // Get last measurement + if ((it_load = nodes_proc_time_.find(it->first)) == nodes_proc_time_.end()) { + nodes_proc_time_.insert(std::pair(it->first, utime + stime)); + continue; + } + + // Calculate node cpu usage + cpu_usage_node = ncpus_ * static_cast(utime + stime - it_load->second) * 100 / (total_period_all); + + // Update parameters + it_load->second = utime + stime; + + // Fill in message + cpu_state_msg_.load_nodes[i].load = cpu_usage_node; + } + // Get cpu temperature stats cpu_state_msg_.temp = freq_cpus_.GetTemperature(temperature_scale_); @@ -557,14 +624,14 @@ int CpuMemMonitor::CollectMemStats() { mem_load_value_ = mem_state_msg_.ram_used / mem_state_msg_.ram_total * 1e+2; - // Go through all the node list and + // Go through all the node list and check memory usage int i = -1; std::map::iterator it; for ( it = nodes_pid_.begin(); it != nodes_pid_.end(); it++ ) { // Increment counter i++; // Look if PID is invalid - if (it->second == -1) + if (it->second <= 0) continue; // Get Memory useage for individual nodes mem_state_msg_.nodes[i].name = it->first; @@ -641,12 +708,9 @@ void CpuMemMonitor::AssertMemStats() { } void CpuMemMonitor::PublishStatsCallback(ros::TimerEvent const &te) { - // Get PIDs of the nodes to monitor - GetPIDs(); - // Get cpu stats if (CollectCPUStats() < 0) { - ROS_FATAL("CPU node unable to get load stats!"); + NODELET_FATAL("CPU node unable to get load stats!"); return; } // Assert cpu stats @@ -657,7 +721,7 @@ void CpuMemMonitor::PublishStatsCallback(ros::TimerEvent const &te) { // Collect Memory stats if (CollectMemStats() < 0) { - ROS_FATAL("Memory node unable to get load stats!"); + NODELET_FATAL("Memory node unable to get load stats!"); return; } // Assert memory stats diff --git a/management/data_bagger/include/data_bagger/data_bagger.h b/management/data_bagger/include/data_bagger/data_bagger.h index 42732f5396..7b50c81e0e 100644 --- a/management/data_bagger/include/data_bagger/data_bagger.h +++ b/management/data_bagger/include/data_bagger/data_bagger.h @@ -54,39 +54,51 @@ class DataBagger : public ff_util::FreeFlyerNodelet { DataBagger(); ~DataBagger(); - bool SetDataToDiskService(ff_msgs::SetDataToDisk::Request &req, + // Service that sets delayed recording settings + bool SetDelayedDataToDiskService(ff_msgs::SetDataToDisk::Request &req, ff_msgs::SetDataToDisk::Response &res); - bool EnableRecordingService(ff_msgs::EnableRecording::Request &req, + // This service enables and disables the delayed recording + bool EnableDelayedRecordingService(ff_msgs::EnableRecording::Request &req, ff_msgs::EnableRecording::Response &res); protected: virtual void Initialize(ros::NodeHandle *nh); - bool ReadParams(); + bool ReadParams(); // Reads data bagger parameters and default profile private: + // Reads all topic names published by the robot void GetTopicNames(); + // Recursively created bag destionation folder bool MakeDir(std::string dir, bool assert_init_fault, std::string &err_msg); + // Gets the date for name generation std::string GetDate(bool with_time); - void FixTopicNamespace(std::string &topic); + // Adds the namespace to topic names + void AddTopicNamespace(std::string &topic); + // Timer for when the robot finishes startup, starts immediate recording void OnStartupTimer(ros::TimerEvent const& event); + // Sets immediate recording settings bool SetImmediateDataToDisk(std::string &err_msg); bool SetDelayedDataToDisk(ff_msgs::DataToDiskState &state, std::string &err_msg); + // Start Recordings void StartDelayedRecording(); void StartImmediateRecording(); + // Clear topics, stop recording void ResetRecorders(bool immediate); + // Update data bagger profile state void GenerateCombinedState(ff_msgs::DataToDiskState *ground_state); + // Publish data bagger state void PublishState(); astrobee_rosbag::Recorder *delayed_recorder_, *immediate_recorder_; diff --git a/management/data_bagger/readme.md b/management/data_bagger/readme.md index f5e27da6e3..5b68552acf 100644 --- a/management/data_bagger/readme.md +++ b/management/data_bagger/readme.md @@ -1,3 +1,7 @@ \page data_bagger Data Bagger -TBD +This node manages recording for the Astrobee robot. This node is mainly used with GDS where different profiles can be easily loaded and managed. + +It distinguishes between two types of recording, immediate and delayed. + * Immediate recording: enabled for the entire duration where the flight software is on. The topics it records are defined in the config file "management/data_bagger.config" and can't be customized during flight. + * Delayed recording: used for data collection, the topics it records can be customized and the recording can start and stop at any time. diff --git a/management/data_bagger/src/data_bagger.cc b/management/data_bagger/src/data_bagger.cc index 71fb1be7c4..3aff0165d1 100644 --- a/management/data_bagger/src/data_bagger.cc +++ b/management/data_bagger/src/data_bagger.cc @@ -61,12 +61,12 @@ void DataBagger::Initialize(ros::NodeHandle *nh) { set_service_ = nh->advertiseService(SERVICE_MANAGEMENT_DATA_BAGGER_SET_DATA_TO_DISK, - &DataBagger::SetDataToDiskService, + &DataBagger::SetDelayedDataToDiskService, this); record_service_ = nh->advertiseService(SERVICE_MANAGEMENT_DATA_BAGGER_ENABLE_RECORDING, - &DataBagger::EnableRecordingService, + &DataBagger::EnableDelayedRecordingService, this); // Timer used to determine when to query ros for the topic list. Timer is one @@ -134,7 +134,7 @@ bool DataBagger::ReadParams() { return false; } - FixTopicNamespace(save_settings.topic_name); + AddTopicNamespace(save_settings.topic_name); if (!topic_entry.GetStr("downlink", &downlink)) { this->AssertFault(ff_util::INITIALIZATION_FAILED, @@ -244,7 +244,7 @@ bool DataBagger::MakeDir(std::string dir, // give the ros bagger the topic gnc/ekf and there is a namespace of bumble, it // will subscribe to /bumble/gnc/ekf but the topic in the bag it records will be // gnc/ekf. -void DataBagger::FixTopicNamespace(std::string &topic) { +void DataBagger::AddTopicNamespace(std::string &topic) { // This function assumes a user doesn't put the namespace in the topic. If // they do, this won't work // Check topic not empty @@ -277,7 +277,7 @@ void DataBagger::OnStartupTimer(ros::TimerEvent const& event) { } } -bool DataBagger::SetDataToDiskService(ff_msgs::SetDataToDisk::Request &req, +bool DataBagger::SetDelayedDataToDiskService(ff_msgs::SetDataToDisk::Request &req, ff_msgs::SetDataToDisk::Response &res) { // Don't allow set data to disk when we are recording if (combined_data_state_.recording) { @@ -311,7 +311,7 @@ bool DataBagger::SetDataToDiskService(ff_msgs::SetDataToDisk::Request &req, return true; } - FixTopicNamespace(setting.topic_name); + AddTopicNamespace(setting.topic_name); recorder_options_delayed_.topics.push_back(setting.topic_name); // TODO(Someone) Need to figure out how to record topics at different @@ -334,7 +334,7 @@ bool DataBagger::SetDataToDiskService(ff_msgs::SetDataToDisk::Request &req, return true; } -bool DataBagger::EnableRecordingService(ff_msgs::EnableRecording::Request &req, +bool DataBagger::EnableDelayedRecordingService(ff_msgs::EnableRecording::Request &req, ff_msgs::EnableRecording::Response &res) { // Check if we are starting a recording or stopping a recording if (req.enable) { diff --git a/mobility/framestore/src/framestore.cc b/mobility/framestore/src/framestore.cc index 98825cf228..5877564135 100644 --- a/mobility/framestore/src/framestore.cc +++ b/mobility/framestore/src/framestore.cc @@ -42,11 +42,16 @@ namespace mobility { class FrameStore : public ff_util::FreeFlyerNodelet { public: - FrameStore() : ff_util::FreeFlyerNodelet(NODE_FRAMESTORE, true) {} + FrameStore() : ff_util::FreeFlyerNodelet() {} ~FrameStore() {} protected: void Initialize(ros::NodeHandle *nh) { + // Set custom config path + char *path; + if ((path = getenv("CUSTOM_CONFIG_DIR")) != NULL) { + config_.SetPath(path); + } // Read the config config_.AddFile("transforms.config"); if (!ReadParams()) diff --git a/scripts/calibrate/readme.md b/scripts/calibrate/readme.md index 2fdf240abd..53eb52ed5e 100644 --- a/scripts/calibrate/readme.md +++ b/scripts/calibrate/readme.md @@ -140,12 +140,16 @@ picoflexx section should be set to the correct value. This will cause the depth camera to run in L2 mode, and produce data on the "extended" topic. -After calibration.launch is run, one should also invoke +After calibration.launch is run, one should also start the pico_proxy +node which will process the extended topic and produce the needed +amplitude data. + +With the current default setup on the robot, this node should start +automatically. If it did not, it can be run with the command: roslaunch $SOURCE_PATH/hardware/pico_driver/launch/pico_proxy.launch -which will process the extended topic and produce the needed amplitude -data. Note that on the robot itself this file is stored at: + Note that on the robot itself this file is stored at: /opt/astrobee/share/pico_driver/launch/pico_proxy.launch diff --git a/scripts/configure.sh b/scripts/configure.sh index 7c648e1ac5..f1b5cdd454 100755 --- a/scripts/configure.sh +++ b/scripts/configure.sh @@ -314,6 +314,16 @@ if [ $native_build == 1 ] ; then echo 'if [[ ":$CMAKE_PREFIX_PATH:" != *":'${cmake_astrobee_path}':"* ]]; then CMAKE_PREFIX_PATH="${CMAKE_PREFIX_PATH:+"$CMAKE_PREFIX_PATH:"}'${cmake_astrobee_path}'"; fi' >> ~/.bashrc } source ~/.bashrc + + shell="$SHELL" + if [[ ${shell} == *"zsh"* ]]; then + echo "Setting .zshrc with environment variables..." + grep -qF 'source /opt/ros/'$ros_version'/setup.zsh' ~/.zshrc || echo 'source /opt/ros/'$ros_version'/setup.zsh' >> ~/.zshrc + grep -qF ${cmake_astrobee_path} ~/.zshrc || { + echo -e '\n' >> ~/.zshrc \ + echo 'if [[ ":$CMAKE_PREFIX_PATH:" != *":'${cmake_astrobee_path}':"* ]]; then CMAKE_PREFIX_PATH="${CMAKE_PREFIX_PATH:+"$CMAKE_PREFIX_PATH:"}'${cmake_astrobee_path}'"; fi' >> ~/.zshrc + } + fi catkin profile add native catkin profile set native diff --git a/scripts/docker/astrobee.Dockerfile b/scripts/docker/astrobee.Dockerfile index 0ef9ee3980..806b838714 100755 --- a/scripts/docker/astrobee.Dockerfile +++ b/scripts/docker/astrobee.Dockerfile @@ -4,7 +4,7 @@ ARG UBUNTU_VERSION=16.04 ARG REMOTE=astrobee -FROM ${REMOTE}/astrobee:base-latest-ubuntu${UBUNTU_VERSION} +FROM ${REMOTE}/astrobee:latest-base-ubuntu${UBUNTU_VERSION} ARG ROS_VERSION=kinetic diff --git a/scripts/docker/astrobee_quick.Dockerfile b/scripts/docker/astrobee_quick.Dockerfile new file mode 100755 index 0000000000..e69878d942 --- /dev/null +++ b/scripts/docker/astrobee_quick.Dockerfile @@ -0,0 +1,42 @@ +# This will set up an Astrobee melodic docker container using the non-NASA +# install instructions. This image builds on top of the base melodic image +# building the code. You must set the docker context to be the repository root +# directory +# +# This dockerfile is an alternative to `astrobee.Dockerfile`. It uses previous +# docker image build artifacts to speed up the catkin build step. If, in the +# future, you were to use a 'quick' astrobee's image as a build cache, be sure +# to add `catkin clean --orphans` so you don't bloat the cache image down the +# line. + +ARG UBUNTU_VERSION=16.04 +ARG REMOTE=astrobee +ARG REMOTE_CACHED=${REMOTE} + +FROM ${REMOTE_CACHED}/astrobee:latest-ubuntu${UBUNTU_VERSION} AS build_cache +FROM ${REMOTE}/astrobee:base-latest-ubuntu${UBUNTU_VERSION} + +ARG ROS_VERSION=kinetic + +COPY --from=build_cache /src/astrobee/build /src/astrobee/build/ +COPY --from=build_cache /src/astrobee/devel /src/astrobee/devel/ +COPY --from=build_cache /src/astrobee/.catkin_tools /src/astrobee/.catkin_tools/ + +COPY . /src/astrobee/src/ + +# reset the timestamps of the files to when they were committed, then cakin will only build new stuff +# https://stackoverflow.com/questions/2458042/restore-a-files-modification-time-in-git +RUN cd /src/astrobee/src/ \ + && \ + for f in $(git ls-tree -r -t --full-name --name-only "HEAD") ; do \ + touch -m -d $(git log --pretty=format:%cI -1 "HEAD" -- "$f") "$f"; \ + done + +RUN . /opt/ros/${ROS_VERSION}/setup.sh \ + && cd /src/astrobee \ + && catkin clean --setup-files \ + && ./src/scripts/configure.sh -l -F -D -T \ + && CMAKE_PREFIX_PATH=$CMAKE_PREFIX_PATH:/src/astrobee/src/cmake \ + && catkin build --status-rate 0.01 + +COPY ./astrobee/resources /opt/astrobee/share/astrobee/resources diff --git a/scripts/docker/build.sh b/scripts/docker/build.sh index 5a93aa1d64..24fb0821b1 100755 --- a/scripts/docker/build.sh +++ b/scripts/docker/build.sh @@ -72,7 +72,7 @@ docker build ${rootdir}/ \ --build-arg UBUNTU_VERSION=${UBUNTU_VERSION} \ --build-arg ROS_VERSION=${ROS_VERSION} \ --build-arg PYTHON=${PYTHON} \ - -t astrobee/astrobee:base-latest-ubuntu${UBUNTU_VERSION} + -t astrobee/astrobee:latest-base-ubuntu${UBUNTU_VERSION} docker build ${rootdir}/ \ -f ${rootdir}/scripts/docker/astrobee.Dockerfile \ --build-arg UBUNTU_VERSION=${UBUNTU_VERSION} \ diff --git a/scripts/docker/readme.md b/scripts/docker/readme.md index 63f1a9b326..87b2430f9d 100644 --- a/scripts/docker/readme.md +++ b/scripts/docker/readme.md @@ -14,6 +14,7 @@ Available docker files: - `astrobee_base.Dockerfile` - Contains installation of all astrobee dependencies the Ubuntu + ROS setup. - `astrobee.Dockerfile` - Builds the astrobee FSW code on top of astrobee_base. +- `astrobee_quick.Dockerfile` - Builds the astrobee FSW code using a previous astrobee image as a build cache. This dramatically speeds up build times for small changes. The Docker files accept the following version args (note that they must match up): @@ -25,6 +26,10 @@ If `UBUNTU_VERSION` is `"16.04"`, `ROS_VERSION` and `PYTHON` must be `"kinetic"` If `UBUNTU_VERSION` is `"18.04"`, `ROS_VERSION` and `PYTHON` must be `"melodic"` and `""` respectively. If `UBUNTU_VERSION` is `"20.04"`, `ROS_VERSION` and `PYTHON` must be `"neotic"` and `"3"` respectively. +The Docker files also accept args to use local or container registry images. + +- `REMOTE` - The repository where the dockerfile should derive its base. Valid values are `astrobee` (the default for local builds) or `ghcr.io/nasa` (the official repository). +- `REMOTE_CACHED` - (Only for `astrobee_quick.dockerfile`, defaults to `${REMOTE}`). The repository for the build cache image. Valid values are `astrobee` (the default for local builds) or `ghcr.io/nasa` (the official repository). ## Building the docker images diff --git a/scripts/docker/test_astrobee.Dockerfile b/scripts/docker/test_astrobee.Dockerfile index 0ce4c93c44..3f9a7c71f3 100644 --- a/scripts/docker/test_astrobee.Dockerfile +++ b/scripts/docker/test_astrobee.Dockerfile @@ -7,6 +7,6 @@ FROM ${REMOTE}/astrobee:latest-ubuntu${UBUNTU_VERSION} # Run tests RUN cd /src/astrobee && catkin build --make-args tests \ - && catkin build --make-args test \ + && catkin build --make-args test -j1\ && { . devel/setup.sh || true; } \ && catkin_test_results build diff --git a/scripts/setup/debians/build_install_debians.sh b/scripts/setup/debians/build_install_debians.sh index 6f166261c6..10b602df64 100755 --- a/scripts/setup/debians/build_install_debians.sh +++ b/scripts/setup/debians/build_install_debians.sh @@ -117,3 +117,24 @@ cd ${DEBIAN_LOC} ./build_openmvg.sh || exit 1 sudo dpkg -i libopenmvg*_amd64.deb || exit 1 +REQUIRED_PKG="rti-dev" +PKG_OK=$(dpkg-query -W --showformat='${Status}\n' $REQUIRED_PKG 2>&1 | grep "install ok installed" || true) +echo Checking for $REQUIRED_PKG: $PKG_OK +if [ "install ok installed" = "$PKG_OK" ]; then + echo "$REQUIRED_PKG exists. Setting up miro and soracore." + + # miro + cd ${DEBIAN_LOC}/miro + sudo mk-build-deps -i -r -t "apt-get --no-install-recommends -y" control + cd ${DEBIAN_LOC} + ./build_miro.sh || exit 1 + sudo dpkg -i libmiro*_amd64.deb || exit 1 + + # soracore + cd ${DEBIAN_LOC}/soracore + sudo mk-build-deps -i -r -t "apt-get --no-install-recommends -y" control + cd ${DEBIAN_LOC} + ./build_soracore.sh || exit 1 + sudo dpkg -i libsoracore*_amd64.deb || exit 1 + +fi \ No newline at end of file diff --git a/scripts/setup/debians/miro/control b/scripts/setup/debians/miro/control index 7b31943b26..d0343b0dda 100644 --- a/scripts/setup/debians/miro/control +++ b/scripts/setup/debians/miro/control @@ -1,7 +1,7 @@ Source: libmiro Priority: optional Maintainer: Brian Coltin -Build-Depends: debhelper (>=9), cmake (>=3.5), rti-dev (>=5.1), libqt4-qt3support (>=4), libace-dev (>=6.3) +Build-Depends: debhelper (>=9), cmake (>=3.5), rti-dev (>=5.1), libqt4-qt3support (>=4) | libqt5widgets5, libace-dev (>=6.3) Standards-Version: 3.9.7 Section: libs Homepage: https://github.com/hhutz/Miro diff --git a/scripts/setup/debians/miro/rules b/scripts/setup/debians/miro/rules index eaf77f368a..985ad16eba 100755 --- a/scripts/setup/debians/miro/rules +++ b/scripts/setup/debians/miro/rules @@ -20,6 +20,6 @@ export DEB_LDFLAGS_MAINT_APPEND = -Wl,--no-as-needed -Wl,-allow-multiple-definit # dh_make generated override targets # This is example for Cmake (See https://bugs.debian.org/641051 ) -#override_dh_auto_configure: -# dh_auto_configure -- # -DCMAKE_LIBRARY_PATH=$(DEB_HOST_MULTIARCH) +override_dh_auto_configure: + dh_auto_configure -- -Dqt_gui_cpp_USE_QT_MAJOR_VERSION=5 diff --git a/scripts/setup/debians/soracore/rules b/scripts/setup/debians/soracore/rules index 29221f9b53..ac7053cc91 100755 --- a/scripts/setup/debians/soracore/rules +++ b/scripts/setup/debians/soracore/rules @@ -20,4 +20,4 @@ export NDDSARCH=$(shell ls $(NDDSHOME)/lib | grep -v jdk) dh $@ --parallel --buildsystem=cmake override_dh_auto_configure: - dh_auto_configure -- -DCMAKE_SHARED_LINKER_FLAGS="-Wl,-allow-multiple-definition" + dh_auto_configure -- -DCMAKE_SHARED_LINKER_FLAGS="-Wl,-allow-multiple-definition" -DMIRO_BUILD_WITH_QT5=true diff --git a/scripts/setup/packages_base_bionic.lst b/scripts/setup/packages_base_bionic.lst index 982b2c902d..231598112a 100644 --- a/scripts/setup/packages_base_bionic.lst +++ b/scripts/setup/packages_base_bionic.lst @@ -25,6 +25,14 @@ libgmp10 libusb-1.0-0 libtinyxml2.6.2v5 libpcl-common1.8 +libpcl-features1.8 +libpcl-filters1.8 +libpcl-kdtree1.8 +libpcl-keypoints1.8 +libpcl-octree1.8 +libpcl-registration1.8 +libpcl-sample-consensus1.8 +libpcl-search1.8 libjson-c3 libargtable2-0 libboost-program-options1.62.0 @@ -103,6 +111,7 @@ libtinyxml-dev libyaml-cpp-dev libusb-1.0-0-dev libpcl-dev +libpcl-conversions-dev libboost-program-options-dev libboost-signals-dev libboost-system-dev diff --git a/scripts/setup/packages_base_focal.lst b/scripts/setup/packages_base_focal.lst index 0a016a8397..132c422f0d 100644 --- a/scripts/setup/packages_base_focal.lst +++ b/scripts/setup/packages_base_focal.lst @@ -23,6 +23,14 @@ libgmp10 libusb-1.0-0 libtinyxml2.6.2v5 libpcl-common1.10 +libpcl-features1.10 +libpcl-filters1.10 +libpcl-kdtree1.10 +libpcl-keypoints1.10 +libpcl-octree1.10 +libpcl-registration1.10 +libpcl-sample-consensus1.10 +libpcl-search1.10 libjson-c4 libargtable2-0 libboost-program-options1.71.0 @@ -101,6 +109,7 @@ libyaml-cpp-dev libusb-1.0-0-dev libvtk7-dev libpcl-dev +libpcl-conversions-dev libboost-program-options-dev libboost-system-dev libboost-chrono-dev diff --git a/scripts/setup/packages_base_xenial.lst b/scripts/setup/packages_base_xenial.lst index 478b33c25e..65e43fdff2 100644 --- a/scripts/setup/packages_base_xenial.lst +++ b/scripts/setup/packages_base_xenial.lst @@ -27,6 +27,14 @@ libgmp10 libusb-1.0-0 libtinyxml2.6.2v5 libpcl-common1.7 +libpcl-features1.7 +libpcl-filters1.7 +libpcl-kdtree1.7 +libpcl-keypoints1.7 +libpcl-octree1.7 +libpcl-registration1.7 +libpcl-sample-consensus1.7 +libpcl-search1.7 libjson-c2 libargtable2-0 libboost-program-options1.58.0 @@ -101,6 +109,7 @@ libtinyxml-dev libyaml-cpp-dev libusb-1.0-0-dev libpcl-dev +libpcl-conversions-dev libboost-program-options-dev libboost-signals-dev libboost-system-dev diff --git a/scripts/setup/packages_desktop_focal.lst b/scripts/setup/packages_desktop_focal.lst index 33dcefc4df..beb101f105 100644 --- a/scripts/setup/packages_desktop_focal.lst +++ b/scripts/setup/packages_desktop_focal.lst @@ -7,3 +7,4 @@ doxygen zlib1g-dev libusb-1.0-0-dev clang-format-8 +python3-rosdep diff --git a/shared/config_reader/include/config_reader/config_reader.h b/shared/config_reader/include/config_reader/config_reader.h index dd95cdf595..bb3ec748c6 100644 --- a/shared/config_reader/include/config_reader/config_reader.h +++ b/shared/config_reader/include/config_reader/config_reader.h @@ -88,6 +88,7 @@ class ConfigReader { bool GetReal(int index, float *val, float min, float max); bool GetReal(const char *exp, double *val, double min, double max); bool GetReal(int index, double *val, double min, double max); + bool IsNumber(const char *exp); ConfigReader *Config() { return config_; } private: @@ -164,7 +165,7 @@ class ConfigReader { bool ReadInt(const char *exp, int *val, int min, int max); bool ReadReal(const char *exp, float *val, float min, float max); bool ReadReal(const char *exp, double *val, double min, double max); - + bool IsNumber(); std::vector files_; // list of config files to load lua_State * l_; // lua interpreter (valid between readFiles() and clear()) diff --git a/shared/config_reader/src/config_reader.cc b/shared/config_reader/src/config_reader.cc index f93d4c8d32..6267dad98e 100644 --- a/shared/config_reader/src/config_reader.cc +++ b/shared/config_reader/src/config_reader.cc @@ -449,6 +449,18 @@ bool ConfigReader::Table::GetReal(int index, double *val, double min, (full_exp_ + '[' + std::to_string(index) + ']').c_str(), val, min, max); } +bool ConfigReader::Table::IsNumber(const char *exp) { + if (!IsInit()) { + return false; + } + + if (!config_->PutObjectOnLuaStack(exp, ref_)) { + return false; + } + + return config_->IsNumber(); +} + //====================================================================// ConfigReader::FileHeader::FileHeader(const FileHeader &fh) { filename_ = fh.filename_; @@ -1096,4 +1108,10 @@ bool ConfigReader::ReadReal(const char *exp, double *val, double min, return true; } +bool ConfigReader::IsNumber() { + // The function PutObjectOnLuaStack() must have been called before + // this, which would populate l_. + return lua_isnumber(l_, -1); +} + } // namespace config_reader diff --git a/shared/ff_util/include/ff_util/ff_names.h b/shared/ff_util/include/ff_util/ff_names.h index 151bb72a60..0ab0f4e85f 100644 --- a/shared/ff_util/include/ff_util/ff_names.h +++ b/shared/ff_util/include/ff_util/ff_names.h @@ -278,6 +278,7 @@ #define SUBSYSTEM_LOCALIZATION "loc" #define NODE_HANDRAIL_DETECT "handrail_detect" +#define NODE_DEPTH_ODOM "depth_odom" #define NODE_OPTICAL_FLOW "of" #define NODE_LOCALIZATION_MANAGER "localization_manager" #define NODE_AR_TAGS "ar_tags" @@ -301,10 +302,12 @@ #define TOPIC_LOCALIZATION_HR_IMAGE "loc/hr/image" #define TOPIC_LOCALIZATION_HR_CLOUD "loc/hr/cloud" #define TOPIC_LOCALIZATION_HR_MARKER "loc/hr/marker" +#define TOPIC_LOCALIZATION_DEPTH_ODOM "loc/depth/odom" #define TOPIC_LOCALIZATION_OF_DEBUG "loc/of/debug_img" #define SERVICE_LOCALIZATION_OF_ENABLE "loc/of/enable" +#define SERVICE_LOCALIZATION_DO_ENABLE "loc/do/enable" #define SERVICE_LOCALIZATION_AR_ENABLE "loc/ar/enable" #define SERVICE_LOCALIZATION_ML_ENABLE "loc/ml/enable" #define SERVICE_LOCALIZATION_HR_ENABLE "loc/hr/enable" @@ -400,6 +403,7 @@ #define TOPIC_HARDWARE_PICOFLEXX_SUFFIX "/points" #define TOPIC_HARDWARE_PICOFLEXX_SUFFIX_EXTENDED "/extended" #define TOPIC_HARDWARE_PICOFLEXX_SUFFIX_DEPTH_IMAGE "/depth_image" +#define TOPIC_HARDWARE_PICOFLEXX_SUFFIX_AMPLITUDE_IMAGE "/amplitude_int" #define TOPIC_HARDWARE_NAME_HAZ_CAM "haz" #define TOPIC_HARDWARE_NAME_PERCH_CAM "perch" #define TOPIC_HARDWARE_SPEED_CAM_CAMERA_IMAGE "hw/speed_cam/camera_image" diff --git a/shared/msg_conversions/include/msg_conversions/msg_conversions.h b/shared/msg_conversions/include/msg_conversions/msg_conversions.h index b40ffc4025..22aa6ab2f7 100644 --- a/shared/msg_conversions/include/msg_conversions/msg_conversions.h +++ b/shared/msg_conversions/include/msg_conversions/msg_conversions.h @@ -25,6 +25,7 @@ #include #include +#include #include #include #include @@ -73,9 +74,12 @@ bool config_read_quat(config_reader::ConfigReader::Table* t, geometry_msgs::Quat bool config_read_vector(config_reader::ConfigReader::Table* t, geometry_msgs::Vector3* vec); bool config_read_vector(config_reader::ConfigReader::Table* t, geometry_msgs::Point* point); +bool SingleBoolTrue(const std::initializer_list& bools); + // Alternative format for loading configs Eigen::Isometry3d LoadEigenTransform(config_reader::ConfigReader& config, const std::string& transform_config_name); double LoadDouble(config_reader::ConfigReader& config, const std::string& config_name); +float LoadFloat(config_reader::ConfigReader& config, const std::string& config_name); int LoadInt(config_reader::ConfigReader& config, const std::string& config_name); bool LoadBool(config_reader::ConfigReader& config, const std::string& config_name); std::string LoadString(config_reader::ConfigReader& config, const std::string& config_name); @@ -83,12 +87,21 @@ void EigenPoseToMsg(const Eigen::Isometry3d& pose, geometry_msgs::Pose& msg_pose void EigenPoseToMsg(const Eigen::Isometry3d& pose, geometry_msgs::Transform& msg_transform); void VariancesToCovDiag(const Eigen::Vector3d& variances, float* const cov_diag); Eigen::Vector3d CovDiagToVariances(const float* const cov_diag); +void EigenPoseCovarianceToMsg(const Eigen::Isometry3d& pose, const Eigen::Matrix& covariance, + geometry_msgs::PoseWithCovarianceStamped& pose_cov_msg); +void EigenPoseCovarianceToMsg(const Eigen::Isometry3d& pose, const Eigen::Matrix& covariance, + geometry_msgs::PoseWithCovariance& pose_cov_msg); template VectorType VectorFromMsg(const MsgVectorType& msg_vector) { return VectorType(msg_vector.x, msg_vector.y, msg_vector.z); } +template +VectorType Vector2dFromMsg(const MsgVectorType& msg_vector) { + return VectorType(msg_vector.x, msg_vector.y); +} + template void VectorToMsg(const VectorType& vector, MsgVectorType& msg_vector) { msg_vector.x = vector.x(); @@ -96,6 +109,12 @@ void VectorToMsg(const VectorType& vector, MsgVectorType& msg_vector) { msg_vector.z = vector.z(); } +template +void Vector2dToMsg(const VectorType& vector, MsgVectorType& msg_vector) { + msg_vector.x = vector.x(); + msg_vector.y = vector.y(); +} + template RotationType RotationFromMsg(const MsgRotationType& msg_rotation) { return RotationType(msg_rotation.w, msg_rotation.x, msg_rotation.y, msg_rotation.z); @@ -108,6 +127,15 @@ void RotationToMsg(const RotationType& rotation, MsgRotationType& msg_rotation) msg_rotation.y = rotation.y(); msg_rotation.z = rotation.z(); } + +template +void EigenCovarianceToMsg(const Eigen::Matrix& covariance, ArrayType& covariance_array) { + for (int i = 0; i < 6; ++i) { + for (int j = 0; j < 6; ++j) { + covariance_array[i*6 + j] = covariance(i, j); + } + } +} } // namespace msg_conversions #endif // MSG_CONVERSIONS_MSG_CONVERSIONS_H_ diff --git a/shared/msg_conversions/src/msg_conversions.cc b/shared/msg_conversions/src/msg_conversions.cc index 34aa0d5fd6..62c4fc3bbc 100644 --- a/shared/msg_conversions/src/msg_conversions.cc +++ b/shared/msg_conversions/src/msg_conversions.cc @@ -230,6 +230,15 @@ bool config_read_vector(config_reader::ConfigReader::Table* t, geometry_msgs::Po return success; } +bool SingleBoolTrue(const std::initializer_list& bools) { + int num_true_bools = 0; + for (const auto bool_value : bools) { + if (bool_value) ++num_true_bools; + if (num_true_bools > 1) return false; + } + return num_true_bools == 1; +} + Eigen::Affine3d ros_pose_to_eigen_transform(const geometry_msgs::Pose& p) { Eigen::Affine3d transform; transform.translation() = Eigen::Vector3d(p.position.x, p.position.y, p.position.z); @@ -256,6 +265,12 @@ Eigen::Isometry3d LoadEigenTransform(config_reader::ConfigReader& config, const return body_T_sensor; } +float LoadFloat(config_reader::ConfigReader& config, const std::string& config_name) { + float val; + if (!config.GetReal(config_name.c_str(), &val)) ROS_FATAL_STREAM("Failed to load " << config_name); + return val; +} + double LoadDouble(config_reader::ConfigReader& config, const std::string& config_name) { double val; if (!config.GetReal(config_name.c_str(), &val)) ROS_FATAL_STREAM("Failed to load " << config_name); @@ -299,4 +314,15 @@ void VariancesToCovDiag(const Eigen::Vector3d& variances, float* const cov_diag) Eigen::Vector3d CovDiagToVariances(const float* const cov_diag) { return Eigen::Vector3d(cov_diag[0], cov_diag[1], cov_diag[2]); } + +void EigenPoseCovarianceToMsg(const Eigen::Isometry3d& pose, const Eigen::Matrix& covariance, + geometry_msgs::PoseWithCovarianceStamped& pose_cov_msg) { + EigenPoseCovarianceToMsg(pose, covariance, pose_cov_msg.pose); +} + +void EigenPoseCovarianceToMsg(const Eigen::Isometry3d& pose, const Eigen::Matrix& covariance, + geometry_msgs::PoseWithCovariance& pose_cov_msg) { + EigenPoseToMsg(pose, pose_cov_msg.pose); + EigenCovarianceToMsg(covariance, pose_cov_msg.covariance); +} } // end namespace msg_conversions diff --git a/simulation/CMakeLists.txt b/simulation/CMakeLists.txt index 09f919d6d5..4a1f602e7a 100644 --- a/simulation/CMakeLists.txt +++ b/simulation/CMakeLists.txt @@ -37,11 +37,16 @@ find_package(catkin2 REQUIRED COMPONENTS ff_util gnc_autocode camera + jsonloader ) # Find packages find_package(gazebo REQUIRED) +# Find jsoncpp +SET(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/../cmake") +find_package(JsonCpp REQUIRED) + # Add gazebo to the link directorries link_directories(${GAZEBO_LIBRARY_DIRS}) @@ -88,6 +93,7 @@ catkin_package( ff_util gnc_autocode camera + jsonloader ) @@ -100,6 +106,7 @@ include_directories( include ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} + ${JSONCPP_INCLUDE_DIRS} ) link_directories(${GAZEBO_LIBRARY_DIRS}) diff --git a/simulation/package.xml b/simulation/package.xml index f1dbeaa26c..c1e44a8aa5 100644 --- a/simulation/package.xml +++ b/simulation/package.xml @@ -29,6 +29,7 @@ ff_util gnc_autocode camera + jsonloader visualization_msgs tf2 tf2_ros @@ -42,6 +43,7 @@ ff_util gnc_autocode camera + jsonloader diff --git a/simulation/readme.md b/simulation/readme.md index e6cc6a0d48..e9e7a826c6 100644 --- a/simulation/readme.md +++ b/simulation/readme.md @@ -1,6 +1,6 @@ \page sim-readme Simulation -This package provides the code required to simulate multiple Free-Flyers operating within the International Space Station (ISS). Our code is written as plugins for Gazebo, an open source robot simulator. In essence the plugins mimic the ros messages and services provided by real hardware, and as a result they act as drop-in replacements for the true hardware. Running a simulation is therefore as simple as loading all flight software subsystems except the hardware drivers, and spawning simulated hardware in stead of real drivers. +This package provides the code required to simulate multiple Free-Flyers operating within the International Space Station (ISS). Our code is written as plugins for Gazebo, an open source robot simulator. In essence the plugins mimic the ros messages and services provided by real hardware, and as a result they act as drop-in replacements for the true hardware. Running a simulation is therefore as simple as loading all flight software subsystems except the hardware drivers, and spawning simulated hardware instead of real drivers. There is once key exception to the rule above: although we do simulate cameras, we do not simulate any vision-based localization. That is, for sparse mapping, optical flow and AR target tracking, instead of running the feature detectors and localization algorithms on the images produced by the simulator, we draw features from the intersection of random rays cast through a virtual image plane with the environment. This approach provides immense performance gains that enables us to run simulations at many times wall clock speed. diff --git a/simulation/running_the_sim.md b/simulation/running_the_sim.md index c7a7befbf9..e503a98c54 100644 --- a/simulation/running_the_sim.md +++ b/simulation/running_the_sim.md @@ -17,6 +17,12 @@ provided by Astrobee flight software: source devel/setup.bash popd +In case of a Zsh session, do + + pushd $BUILD_PATH + source devel/setup.zsh + popd + After this command has completed, you should be able to run `ros` commands from any directory in your Linux filesystem. diff --git a/simulation/src/gazebo_sensor_plugin_sci_cam/gazebo_sensor_plugin_sci_cam.cc b/simulation/src/gazebo_sensor_plugin_sci_cam/gazebo_sensor_plugin_sci_cam.cc index f6e13492a4..334ba4653d 100644 --- a/simulation/src/gazebo_sensor_plugin_sci_cam/gazebo_sensor_plugin_sci_cam.cc +++ b/simulation/src/gazebo_sensor_plugin_sci_cam/gazebo_sensor_plugin_sci_cam.cc @@ -22,6 +22,11 @@ // FSW includes #include +// JSON includes +#include +#include +#include + // Sensor plugin interface #include @@ -156,30 +161,6 @@ class GazeboSensorPluginSciCam : public FreeFlyerSensorPlugin { } } - // Out of string: "{"name": "turnOnContinuousPictureTaking"}" - // collect the part in the second set of quotes. - // Some honest json parsing could be used here. - std::string parseJsonStr(std::string const& json_str) { - size_t start = 0; - size_t colon_pos = json_str.find(":", start); - if (colon_pos == std::string::npos) { - return ""; - } - size_t quote1_pos = json_str.find("\"", colon_pos + 1); - if (quote1_pos == std::string::npos) { - return ""; - } - size_t quote2_pos = json_str.find("\"", quote1_pos + 1); - - if (quote2_pos == std::string::npos) { - return ""; - } - - std::string parsed = json_str.substr(quote1_pos + 1, quote2_pos - quote1_pos - 1); - - return parsed; - } - // Called when a dds command is received. Process only guest science // sci cam control commands. void CmdCallback(ff_msgs::CommandStamped const& cmd) { @@ -199,7 +180,7 @@ class GazeboSensorPluginSciCam : public FreeFlyerSensorPlugin { return; } - std::string app_name = cmd.args[0].s.data(); + std::string app_name = cmd.args[0].s.data(); // Process only sci cam commands if (app_name != "gov.nasa.arc.irg.astrobee.sci_cam_image") { @@ -209,22 +190,40 @@ class GazeboSensorPluginSciCam : public FreeFlyerSensorPlugin { std::string json_str = cmd.args[1].s.data(); ROS_INFO_STREAM("Received command: " << json_str); - std::string action = parseJsonStr(json_str); - if (action == "") + // Convert string into a json object + Json::Value cmd_obj; + if (!jsonloader::LoadData(json_str, &cmd_obj)) { + ROS_ERROR_STREAM("Unable to convert command " << json_str << " to json."); + return; + } + + // Check to make sure command name exists + if (!cmd_obj.isMember("name") || !cmd_obj["name"].isString()) { + ROS_ERROR_STREAM(json_str << " doesn't contain name for the command."); return; + } + + std::string action = cmd_obj["name"].asString(); // Record the desired intention. Use a lock. { const std::lock_guard lock(sci_cam_image_lock); - if (action == "takeSinglePicture") { + if (action == "takePicture") { takeSinglePicture_ = true; continuousPictureTaking_ = false; - } else if (action == "turnOnContinuousPictureTaking") { - takeSinglePicture_ = false; - continuousPictureTaking_ = true; - } else if (action == "turnOffContinuousPictureTaking") { - takeSinglePicture_ = false; - continuousPictureTaking_ = false; + } else if (action == "setContinuousPictureTaking") { + if (cmd_obj.isMember("continuous") && cmd_obj["continuous"].isBool()) { + if (cmd_obj["continuous"].asBool()) { + takeSinglePicture_ = false; + continuousPictureTaking_ = true; + } else { + takeSinglePicture_ = false; + continuousPictureTaking_ = false; + } + } else { + ROS_ERROR_STREAM("Got set continuous picture taking command but it" << + " didn't contain the continuous argument."); + } } else { ROS_FATAL_STREAM("Unknown sci_cam command: " << action); } diff --git a/submodules/android b/submodules/android index 4fa00c148c..a8560ab027 160000 --- a/submodules/android +++ b/submodules/android @@ -1 +1 @@ -Subproject commit 4fa00c148c514183637010cbf8a87a08beff0bbc +Subproject commit a8560ab0270ac281d8eadeb48645f4224582985e diff --git a/submodules/platform b/submodules/platform index 8e29458611..c02084b876 160000 --- a/submodules/platform +++ b/submodules/platform @@ -1 +1 @@ -Subproject commit 8e29458611ed62c1b554a74b5796f3c67ed068b1 +Subproject commit c02084b87613621531414ab9d7d545326a047812 diff --git a/tools/calibration/CMakeLists.txt b/tools/calibration/CMakeLists.txt index dd48d3a5e9..ecaa6e19a5 100644 --- a/tools/calibration/CMakeLists.txt +++ b/tools/calibration/CMakeLists.txt @@ -27,6 +27,7 @@ find_package(catkin2 REQUIRED COMPONENTS localization_common msg_conversions optimization_common + vision_common ) # Find OpenCV3 @@ -40,7 +41,7 @@ find_package(Ceres REQUIRED) catkin_package( INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} ${CERES_LIBRARIES} ${OpenCV_LIBRARIES} - CATKIN_DEPENDS camera localization_common msg_conversions optimization_common + CATKIN_DEPENDS camera localization_common msg_conversions optimization_common vision_common ) ########### @@ -57,7 +58,6 @@ include_directories( # Declare C++ libraries add_library(${PROJECT_NAME} - src/camera_utilities.cc src/parameter_reader.cc src/utilities.cc ) @@ -107,38 +107,6 @@ install(CODE "execute_process( if(CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) - add_rostest_gtest(test_ransac_pnp - test/test_ransac_pnp.test - test/test_ransac_pnp.cc - test/test_utilities.cc - ) - target_link_libraries(test_ransac_pnp - ${PROJECT_NAME} ${catkin_LIBRARIES} - ) - add_rostest_gtest(test_reprojection_pose_estimate - test/test_reprojection_pose_estimate.test - test/test_reprojection_pose_estimate.cc - test/test_utilities.cc - ) - target_link_libraries(test_reprojection_pose_estimate - ${PROJECT_NAME} ${catkin_LIBRARIES} - ) - add_rostest_gtest(test_pnp - test/test_pnp.test - test/test_pnp.cc - test/test_utilities.cc - ) - target_link_libraries(test_pnp - ${PROJECT_NAME} ${catkin_LIBRARIES} - ) - add_rostest_gtest(test_camera_utilities - test/test_camera_utilities.test - test/test_camera_utilities.cc - test/test_utilities.cc - ) - target_link_libraries(test_camera_utilities - ${PROJECT_NAME} ${catkin_LIBRARIES} - ) add_rostest_gtest(test_camera_target_based_intrinsics_calibrator test/test_camera_target_based_intrinsics_calibrator.test test/test_camera_target_based_intrinsics_calibrator.cc diff --git a/tools/calibration/include/calibration/camera_target_based_intrinsics_calibrator.h b/tools/calibration/include/calibration/camera_target_based_intrinsics_calibrator.h index 505683dc4b..26daf483ae 100644 --- a/tools/calibration/include/calibration/camera_target_based_intrinsics_calibrator.h +++ b/tools/calibration/include/calibration/camera_target_based_intrinsics_calibrator.h @@ -28,6 +28,7 @@ #include #include #include +#include #include #include @@ -80,7 +81,7 @@ bool CameraTargetBasedIntrinsicsCalibrator::EstimateInitialTargetPose const Eigen::Matrix3d initial_intrinsics = optimization_common::Intrinsics(initial_state_parameters.focal_lengths, initial_state_parameters.principal_points); for (const auto& correspondences : correspondences_set) { - const auto camera_T_target = ReprojectionPoseEstimate( + const auto camera_T_target = vision_common::ReprojectionPoseEstimate( correspondences.image_points, correspondences.points_3d, initial_state_parameters.focal_lengths, initial_state_parameters.principal_points, initial_state_parameters.distortion, params_.reprojection_pose_estimate); diff --git a/tools/calibration/include/calibration/camera_target_based_intrinsics_calibrator_params.h b/tools/calibration/include/calibration/camera_target_based_intrinsics_calibrator_params.h index 72fda66941..fcadde48d7 100644 --- a/tools/calibration/include/calibration/camera_target_based_intrinsics_calibrator_params.h +++ b/tools/calibration/include/calibration/camera_target_based_intrinsics_calibrator_params.h @@ -18,9 +18,9 @@ #ifndef CALIBRATION_CAMERA_TARGET_BASED_INTRINSICS_CALIBRATOR_PARAMS_H_ #define CALIBRATION_CAMERA_TARGET_BASED_INTRINSICS_CALIBRATOR_PARAMS_H_ -#include -#include #include +#include +#include #include @@ -29,8 +29,8 @@ namespace calibration { struct CameraTargetBasedIntrinsicsCalibratorParams { - OptimizationParams optimization; - ReprojectionPoseEstimateParams reprojection_pose_estimate; + optimization_common::OptimizationParams optimization; + vision_common::ReprojectionPoseEstimateParams reprojection_pose_estimate; bool calibrate_focal_lengths; bool calibrate_principal_points; bool calibrate_distortion; diff --git a/tools/calibration/include/calibration/parameter_reader.h b/tools/calibration/include/calibration/parameter_reader.h index 4011afc578..56e1a17b77 100644 --- a/tools/calibration/include/calibration/parameter_reader.h +++ b/tools/calibration/include/calibration/parameter_reader.h @@ -20,9 +20,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include @@ -34,15 +34,16 @@ void LoadRunCalibratorParams(config_reader::ConfigReader& config, RunCalibratorP void LoadCameraTargetBasedIntrinsicsCalibratorParams(config_reader::ConfigReader& config, CameraTargetBasedIntrinsicsCalibratorParams& params); -void LoadReprojectionPoseEstimateParams(config_reader::ConfigReader& config, ReprojectionPoseEstimateParams& params); +void LoadReprojectionPoseEstimateParams(config_reader::ConfigReader& config, + vision_common::ReprojectionPoseEstimateParams& params); -void LoadOptimizationParams(config_reader::ConfigReader& config, OptimizationParams& params, +void LoadOptimizationParams(config_reader::ConfigReader& config, optimization_common::OptimizationParams& params, const std::string& prefix = ""); void LoadSolverOptions(config_reader::ConfigReader& config, ceres::Solver::Options& solver_options, const std::string& prefix = ""); -void LoadRansacPnPParams(config_reader::ConfigReader& config, RansacPnPParams& params); +void LoadRansacPnPParams(config_reader::ConfigReader& config, vision_common::RansacPnPParams& params); } // namespace calibration #endif // CALIBRATION_PARAMETER_READER_H_ diff --git a/tools/calibration/include/calibration/utilities.h b/tools/calibration/include/calibration/utilities.h index bd3f99cb75..d00d321010 100644 --- a/tools/calibration/include/calibration/utilities.h +++ b/tools/calibration/include/calibration/utilities.h @@ -18,11 +18,11 @@ #ifndef CALIBRATION_UTILITIES_H_ #define CALIBRATION_UTILITIES_H_ -#include #include #include #include #include +#include #include @@ -73,7 +73,7 @@ void SaveReprojectionImage(const std::vector& image_points, const Eigen::Vector3d& point_3d = points_3d[i]; const Eigen::Vector3d camera_t_target_point = pose * point_3d; const Eigen::Vector2d projected_image_point = - Project3dPointToImageSpaceWithDistortion(camera_t_target_point, intrinsics, distortion); + vision_common::Project3dPointToImageSpaceWithDistortion(camera_t_target_point, intrinsics, distortion); const Eigen::Vector2d error = (image_point - projected_image_point); const double error_norm = error.norm(); const cv::Point2i rounded_image_point(std::round(image_point.x()), std::round(image_point.y())); @@ -118,8 +118,8 @@ void SaveReprojectionFromAllTargetsImage(const std::vector& c const auto& image_point = match_set.image_points[j]; const auto& target_point = match_set.points_3d[j]; const Eigen::Vector3d camera_t_target_point = camera_T_target * target_point; - const Eigen::Vector2d projected_image_point = - Project3dPointToImageSpaceWithDistortion(camera_t_target_point, intrinsics, distortion); + const Eigen::Vector2d projected_image_point = vision_common::Project3dPointToImageSpaceWithDistortion( + camera_t_target_point, intrinsics, distortion); const Eigen::Vector2d error = (image_point - projected_image_point); const double error_norm = error.norm(); if (error_norm > max_error_norm) max_error_norm = error_norm; @@ -147,8 +147,8 @@ void SaveReprojectionFromAllTargetsImage(const std::vector& c const auto& image_point = match_set.image_points[j]; const auto& target_point = match_set.points_3d[j]; const Eigen::Vector3d camera_t_target_point = camera_T_target * target_point; - const Eigen::Vector2d projected_image_point = - Project3dPointToImageSpaceWithDistortion(camera_t_target_point, intrinsics, distortion); + const Eigen::Vector2d projected_image_point = vision_common::Project3dPointToImageSpaceWithDistortion( + camera_t_target_point, intrinsics, distortion); const Eigen::Vector2d error = (image_point - projected_image_point); const double error_norm = error.norm(); const cv::Point2i rounded_image_point(std::round(image_point.x()), std::round(image_point.y())); diff --git a/tools/calibration/package.xml b/tools/calibration/package.xml index ff430597e2..ed5b9b285d 100644 --- a/tools/calibration/package.xml +++ b/tools/calibration/package.xml @@ -19,9 +19,11 @@ localization_common msg_conversions optimization_common + vision_common eigen_conversions camera localization_common msg_conversions optimization_common + vision_common diff --git a/tools/calibration/src/parameter_reader.cc b/tools/calibration/src/parameter_reader.cc index 4cf4070924..be393a48a6 100644 --- a/tools/calibration/src/parameter_reader.cc +++ b/tools/calibration/src/parameter_reader.cc @@ -24,6 +24,8 @@ namespace calibration { namespace mc = msg_conversions; +namespace oc = optimization_common; +namespace vc = vision_common; void LoadRunCalibratorParams(config_reader::ConfigReader& config, RunCalibratorParams& params) { LoadCameraTargetBasedIntrinsicsCalibratorParams(config, params.camera_target_based_intrinsics_calibrator); @@ -56,7 +58,8 @@ void LoadCameraTargetBasedIntrinsicsCalibratorParams(config_reader::ConfigReader params.image_size = Eigen::Vector2i(image_width, image_height); } -void LoadReprojectionPoseEstimateParams(config_reader::ConfigReader& config, ReprojectionPoseEstimateParams& params) { +void LoadReprojectionPoseEstimateParams(config_reader::ConfigReader& config, + vc::ReprojectionPoseEstimateParams& params) { LoadOptimizationParams(config, params.optimization, "reprojection_"); LoadRansacPnPParams(config, params.ransac_pnp); params.optimize_estimate = mc::LoadBool(config, "reprojection_optimize_estimate"); @@ -85,14 +88,14 @@ void LoadSolverOptions(config_reader::ConfigReader& config, ceres::Solver::Optio solver_options.parameter_tolerance = mc::LoadDouble(config, prefix + "parameter_tolerance"); } -void LoadOptimizationParams(config_reader::ConfigReader& config, OptimizationParams& params, +void LoadOptimizationParams(config_reader::ConfigReader& config, oc::OptimizationParams& params, const std::string& prefix) { LoadSolverOptions(config, params.solver_options, prefix); params.verbose = mc::LoadBool(config, prefix + "verbose_optimization"); params.huber_loss = mc::LoadDouble(config, prefix + "huber_loss"); } -void LoadRansacPnPParams(config_reader::ConfigReader& config, RansacPnPParams& params) { +void LoadRansacPnPParams(config_reader::ConfigReader& config, vc::RansacPnPParams& params) { params.max_inlier_threshold = mc::LoadDouble(config, "ransac_max_inlier_threshold"); params.num_iterations = mc::LoadInt(config, "ransac_num_iterations"); params.min_num_inliers = mc::LoadInt(config, "ransac_min_num_inliers"); diff --git a/tools/calibration/test/test_camera_target_based_intrinsics_calibrator.cc b/tools/calibration/test/test_camera_target_based_intrinsics_calibrator.cc index 606b6dbe48..615b18033a 100644 --- a/tools/calibration/test/test_camera_target_based_intrinsics_calibrator.cc +++ b/tools/calibration/test/test_camera_target_based_intrinsics_calibrator.cc @@ -22,17 +22,16 @@ #include #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include #include namespace ca = calibration; namespace lc = localization_common; -namespace oc = optimization_common; +namespace vc = vision_common; TEST(CameraTargetBasedIntrinsicsCalibratorTester, EvenlySpacedTargetsIdentityDistortionWithNoise) { auto params = ca::DefaultCameraTargetBasedIntrinsicsCalibratorParams(); @@ -55,9 +54,9 @@ TEST(CameraTargetBasedIntrinsicsCalibratorTester, EvenlySpacedTargetsIdentityDis const auto noisy_state_parameters = ca::AddNoiseToStateParameters(true_state_parameters, focal_lengths_stddev, principal_points_stddev, distortion_stddev); ca::StateParameters calibrated_state_parameters; - const auto match_sets = ca::EvenlySpacedTargetMatchSets( + const auto match_sets = ca::EvenlySpacedTargetMatchSets( num_pose_rows, num_pose_cols, num_pose_y_levels, num_target_points_per_row_and_col, intrinsics); - ca::CameraTargetBasedIntrinsicsCalibrator calibrator(params); + ca::CameraTargetBasedIntrinsicsCalibrator calibrator(params); ca::StateParametersCovariances covariances; ASSERT_TRUE(calibrator.Calibrate(match_sets, noisy_state_parameters, calibrated_state_parameters, covariances)); ASSERT_PRED2(lc::MatrixEquality<2>, true_state_parameters.focal_lengths.matrix(), @@ -82,14 +81,14 @@ TEST(CameraTargetBasedIntrinsicsCalibratorTester, EvenlySpacedTargetsFovDistorti ca::StateParameters true_state_parameters; true_state_parameters.focal_lengths = lc::FocalLengths(intrinsics); true_state_parameters.principal_points = lc::PrincipalPoints(intrinsics); - true_state_parameters.distortion = ca::RandomFovDistortion(); + true_state_parameters.distortion = vc::RandomFovDistortion(); const auto noisy_state_parameters = ca::AddNoiseToStateParameters(true_state_parameters, focal_lengths_stddev, principal_points_stddev, distortion_stddev); ca::StateParameters calibrated_state_parameters; - const auto match_sets = ca::EvenlySpacedTargetMatchSets( + const auto match_sets = ca::EvenlySpacedTargetMatchSets( num_pose_rows, num_pose_cols, num_pose_y_levels, num_target_points_per_row_and_col, intrinsics, true_state_parameters.distortion); - ca::CameraTargetBasedIntrinsicsCalibrator calibrator(params); + ca::CameraTargetBasedIntrinsicsCalibrator calibrator(params); ca::StateParametersCovariances covariances; ASSERT_TRUE(calibrator.Calibrate(match_sets, noisy_state_parameters, calibrated_state_parameters, covariances)); ASSERT_PRED2(lc::MatrixEquality<2>, true_state_parameters.focal_lengths.matrix(), @@ -117,14 +116,14 @@ TEST(CameraTargetBasedIntrinsicsCalibratorTester, EvenlySpacedTargetsRadDistorti ca::StateParameters true_state_parameters; true_state_parameters.focal_lengths = lc::FocalLengths(intrinsics); true_state_parameters.principal_points = lc::PrincipalPoints(intrinsics); - true_state_parameters.distortion = ca::RandomRadDistortion(); + true_state_parameters.distortion = vc::RandomRadDistortion(); const auto noisy_state_parameters = ca::AddNoiseToStateParameters(true_state_parameters, focal_lengths_stddev, principal_points_stddev, distortion_stddev); ca::StateParameters calibrated_state_parameters; - const auto match_sets = ca::EvenlySpacedTargetMatchSets( + const auto match_sets = ca::EvenlySpacedTargetMatchSets( num_pose_rows, num_pose_cols, num_pose_y_levels, num_target_points_per_row_and_col, intrinsics, true_state_parameters.distortion); - ca::CameraTargetBasedIntrinsicsCalibrator calibrator(params); + ca::CameraTargetBasedIntrinsicsCalibrator calibrator(params); ca::StateParametersCovariances covariances; ASSERT_TRUE(calibrator.Calibrate(match_sets, noisy_state_parameters, calibrated_state_parameters, covariances)); ASSERT_PRED2(lc::MatrixEquality<2>, true_state_parameters.focal_lengths.matrix(), @@ -151,14 +150,14 @@ TEST(CameraTargetBasedIntrinsicsCalibratorTester, EvenlySpacedTargetsRadTanDisto ca::StateParameters true_state_parameters; true_state_parameters.focal_lengths = lc::FocalLengths(intrinsics); true_state_parameters.principal_points = lc::PrincipalPoints(intrinsics); - true_state_parameters.distortion = ca::RandomRadTanDistortion(); + true_state_parameters.distortion = vc::RandomRadTanDistortion(); const auto noisy_state_parameters = ca::AddNoiseToStateParameters(true_state_parameters, focal_lengths_stddev, principal_points_stddev, distortion_stddev); ca::StateParameters calibrated_state_parameters; - const auto match_sets = ca::EvenlySpacedTargetMatchSets( + const auto match_sets = ca::EvenlySpacedTargetMatchSets( num_pose_rows, num_pose_cols, num_pose_y_levels, num_target_points_per_row_and_col, intrinsics, true_state_parameters.distortion); - ca::CameraTargetBasedIntrinsicsCalibrator calibrator(params); + ca::CameraTargetBasedIntrinsicsCalibrator calibrator(params); ca::StateParametersCovariances covariances; ASSERT_TRUE(calibrator.Calibrate(match_sets, noisy_state_parameters, calibrated_state_parameters, covariances)); ASSERT_PRED2(lc::MatrixEquality<2>, true_state_parameters.focal_lengths.matrix(), diff --git a/tools/calibration/test/test_utilities.cc b/tools/calibration/test/test_utilities.cc index 49e98fa90f..4b788db951 100644 --- a/tools/calibration/test/test_utilities.cc +++ b/tools/calibration/test/test_utilities.cc @@ -18,50 +18,18 @@ #include "test_utilities.h" // NOLINT -#include #include #include namespace calibration { namespace lc = localization_common; - -OptimizationParams DefaultOptimizationParams() { - OptimizationParams params; - params.solver_options.linear_solver_type = ceres::ITERATIVE_SCHUR; - params.solver_options.use_explicit_schur_complement = false; - params.solver_options.max_num_iterations = 100; - params.solver_options.function_tolerance = 1e-8; - params.solver_options.parameter_tolerance = 1e-8; - params.verbose = false; - params.huber_loss = 1.345; - return params; -} - -RansacPnPParams DefaultRansacPnPParams() { - RansacPnPParams params; - params.max_inlier_threshold = 3; - params.num_iterations = 100; - params.min_num_inliers = 4; - // TODO(rsoussan): Change this to cv::SOLVEPNP_IPPE when available. - // Currenty cv::SOLVEPNP_P3P and cv::SOLVEPNP_ITERATIVE lead to significant errors even with perfect data - params.pnp_method = cv::SOLVEPNP_EPNP; - return params; -} - -ReprojectionPoseEstimateParams DefaultReprojectionPoseEstimateParams() { - ReprojectionPoseEstimateParams params; - params.optimization = DefaultOptimizationParams(); - params.ransac_pnp = DefaultRansacPnPParams(); - params.optimize_estimate = true; - params.max_inlier_threshold = 3.0; - return params; -} +namespace vc = vision_common; CameraTargetBasedIntrinsicsCalibratorParams DefaultCameraTargetBasedIntrinsicsCalibratorParams() { CameraTargetBasedIntrinsicsCalibratorParams params; - params.optimization = DefaultOptimizationParams(); - params.reprojection_pose_estimate = DefaultReprojectionPoseEstimateParams(); + params.optimization = vc::DefaultOptimizationParams(); + params.reprojection_pose_estimate = vc::DefaultReprojectionPoseEstimateParams(); params.calibrate_focal_lengths = true; params.calibrate_principal_points = true; params.calibrate_distortion = true; @@ -79,138 +47,6 @@ CameraTargetBasedIntrinsicsCalibratorParams DefaultCameraTargetBasedIntrinsicsCa return params; } -Eigen::VectorXd RandomFovDistortion() { - Eigen::VectorXd distortion(1); - distortion[0] = lc::RandomDouble(0.1, 0.3); - return distortion; -} - -Eigen::VectorXd RandomRadDistortion() { - Eigen::VectorXd distortion(2); - distortion[0] = lc::RandomDouble(-0.1, 0.1); - distortion[1] = lc::RandomDouble(-0.1, 0.1); - return distortion; -} - -Eigen::VectorXd RandomRadTanDistortion() { - Eigen::VectorXd distortion(4); - distortion[0] = lc::RandomDouble(-0.1, 0.1); - distortion[1] = lc::RandomDouble(-0.1, 0.1); - distortion[2] = lc::RandomDouble(0, 0.1); - distortion[3] = lc::RandomDouble(0, 0.1); - return distortion; -} - -std::vector TargetPoints(const int points_per_row, const int points_per_col, const double row_spacing, - const double col_spacing) { - Eigen::Vector3d target_center(points_per_col * col_spacing / 2.0, points_per_row * row_spacing / 2.0, 0.0); - std::vector target_points; - for (int i = 0; i < points_per_col; ++i) { - for (int j = 0; j < points_per_row; ++j) { - // Center target points about (0,0) - target_points.emplace_back(Eigen::Vector3d(i * col_spacing, j * row_spacing, 0) - target_center); - } - } - return target_points; -} - -std::vector RandomFrontFacingPoints(const int num_points) { - std::vector points; - for (int i = 0; i < num_points; ++i) { - points.emplace_back(RandomFrontFacingPoint()); - } - return points; -} - -Eigen::Vector3d RandomFrontFacingPoint() { - static constexpr double rho_min = 1.0; - static constexpr double rho_max = 3.0; - static constexpr double phi_min = -25.0; - static constexpr double phi_max = 25.0; - static constexpr double z_rho_scale = 0.5; - return RandomFrontFacingPoint(rho_min, rho_max, phi_min, phi_max, z_rho_scale); -} - -Eigen::Vector3d RandomFrontFacingPoint(const double rho_min, const double rho_max, const double phi_min, - const double phi_max, const double z_rho_scale) { - const double rho = lc::RandomDouble(rho_min, rho_max); - const double phi = lc::RandomDouble(phi_min, phi_max); - const double z = lc::RandomDouble(-1.0 * z_rho_scale * rho, z_rho_scale * rho); - // Z and x are swapped so z defines distance from camera rather than height - const Eigen::Vector3d tmp = lc::CylindricalToCartesian(Eigen::Vector3d(rho, phi, z)); - const Eigen::Vector3d random_point(tmp.z(), tmp.y(), tmp.x()); - return random_point; -} - -Eigen::Isometry3d RandomFrontFacingPose() { - static constexpr double rho_min = 1.0; - static constexpr double rho_max = 3.0; - static constexpr double phi_min = -25.0; - static constexpr double phi_max = 25.0; - static constexpr double z_rho_scale = 0.5; - - // Pitch acts like yaw since z axis points outwards in camera frame - static constexpr double yaw_min = -10.0; - static constexpr double yaw_max = 10.0; - static constexpr double pitch_min = -45; - static constexpr double pitch_max = 45; - static constexpr double roll_min = -10; - static constexpr double roll_max = 10; - - return RandomFrontFacingPose(rho_min, rho_max, phi_min, phi_max, z_rho_scale, yaw_min, yaw_max, pitch_min, pitch_max, - roll_min, roll_max); -} - -Eigen::Isometry3d RandomFrontFacingPose(const double rho_min, const double rho_max, const double phi_min, - const double phi_max, const double z_rho_scale, const double yaw_min, - const double yaw_max, const double pitch_min, const double pitch_max, - const double roll_min, const double roll_max) { - const Eigen::Vector3d translation = RandomFrontFacingPoint(rho_min, rho_max, phi_min, phi_max, z_rho_scale); - const double yaw = lc::RandomDouble(yaw_min, yaw_max); - const double pitch = lc::RandomDouble(pitch_min, pitch_max); - const double roll = lc::RandomDouble(roll_min, roll_max); - const Eigen::Matrix3d rotation = lc::RotationFromEulerAngles(yaw, pitch, roll); - return lc::Isometry3d(translation, rotation); -} - -std::vector EvenlySpacedTargetPoses(const int num_rows, const int num_cols, const int num_y_levels) { - // Pitch acts like yaw since z axis points out of camera frame - const lc::Sampler pitch_sampler(-15.0, 15.0, num_cols); - const lc::Sampler roll_sampler(-15.0, 15.0, num_cols); - const lc::Sampler yaw_sampler(-15.0, 15.0, num_cols); - - // Cylindrical coordinates for translation - const lc::Sampler rho_sampler(1.0, 3.0, num_rows); - constexpr double phi = 0.0; - - // Use smaller y_rho scale factor since image is shorter than it is wide - constexpr double y_rho_scale = 0.235; - constexpr double z_rho_scale = 0.5; - - std::vector poses; - for (int i = 0; i < num_rows; ++i) { - const double rho = rho_sampler.Sample(i); - const lc::Sampler z_sampler(-1.0 * rho * z_rho_scale, rho * z_rho_scale, num_cols); - for (int j = 0; j < num_cols; ++j) { - const double pitch = pitch_sampler.Sample(j); - const double roll = roll_sampler.Sample(j); - const double yaw = yaw_sampler.Sample(j); - const double z = z_sampler.Sample(j); - // Z and x are swapped so z defines distance from camera rather than height - const Eigen::Vector3d tmp = lc::CylindricalToCartesian(Eigen::Vector3d(rho, phi, z)); - const Eigen::Matrix3d rotation = lc::RotationFromEulerAngles(yaw, pitch, roll); - const lc::Sampler y_sampler(-1.0 * rho * y_rho_scale, rho * y_rho_scale, num_y_levels); - for (int k = 0; k < num_y_levels; ++k) { - const double y = y_sampler.Sample(k); - const Eigen::Vector3d translation(tmp.z(), y, tmp.x()); - poses.emplace_back(lc::Isometry3d(translation, rotation)); - } - } - } - - return poses; -} - StateParameters AddNoiseToStateParameters(const StateParameters& state_parameters, const double focal_lengths_stddev, const double principal_points_stddev, const double distortion_stddev, const bool fov) { diff --git a/tools/calibration/test/test_utilities.h b/tools/calibration/test/test_utilities.h index 7d2d6f1dbc..050879f6c7 100644 --- a/tools/calibration/test/test_utilities.h +++ b/tools/calibration/test/test_utilities.h @@ -21,56 +21,16 @@ #include #include #include -#include -#include -#include #include #include #include +#include #include namespace calibration { -OptimizationParams DefaultOptimizationParams(); - -RansacPnPParams DefaultRansacPnPParams(); - -ReprojectionPoseEstimateParams DefaultReprojectionPoseEstimateParams(); - CameraTargetBasedIntrinsicsCalibratorParams DefaultCameraTargetBasedIntrinsicsCalibratorParams(); -Eigen::VectorXd RandomFovDistortion(); - -Eigen::VectorXd RandomRadDistortion(); - -Eigen::VectorXd RandomRadTanDistortion(); - -// Samples in cylindrical coordinates for pose translation to keep pose in view frustrum. -// Samples z using scaled rho value to prevent large z vals with small rho values -// that may move the pose out of the view frustrum. -Eigen::Isometry3d RandomFrontFacingPose(const double rho_min, const double rho_max, const double phi_min, - const double phi_max, const double z_rho_scale, const double yaw_min, - const double yaw_max, const double pitch_min, const double pitch_max, - const double roll_min, const double roll_max); - -Eigen::Isometry3d RandomFrontFacingPose(); - -// Spaced out poses for targets which when projected into image space cover -// the image well with target points. Poses are sampled for each row/col combination -// and evenly spaced in cylindrical coordinates -std::vector EvenlySpacedTargetPoses(const int num_rows = 3, const int num_cols = 5, - const int num_y_levels = 2); - -std::vector TargetPoints(const int points_per_row, const int points_per_col, - const double row_spacing = 0.1, const double col_spacing = 0.1); - -std::vector RandomFrontFacingPoints(const int num_points); - -Eigen::Vector3d RandomFrontFacingPoint(); - -Eigen::Vector3d RandomFrontFacingPoint(const double rho_min, const double rho_max, const double phi_min, - const double phi_max, const double z_rho_scale); - template std::vector RandomTargetMatchSets(const int num_match_sets, const int num_target_points_per_row_and_col, const Eigen::Matrix3d& intrinsics, @@ -83,48 +43,15 @@ std::vector EvenlySpacedTargetMatchSets(const int num_pose_rows, const const Eigen::Matrix3d& intrinsics, const Eigen::VectorXd& distortion = Eigen::VectorXd()); -template -class RegistrationCorrespondences { - public: - RegistrationCorrespondences(const Eigen::Isometry3d& camera_T_target, const Eigen::Matrix3d& intrinsics, - const std::vector& target_t_target_point, - const Eigen::VectorXd& distortion = Eigen::VectorXd()); - - const localization_common::ImageCorrespondences& correspondences() const { return correspondences_; } - - const Eigen::Isometry3d& camera_T_target() const { return camera_T_target_; } - - const Eigen::Matrix3d& intrinsics() const { return intrinsics_; } - - private: - localization_common::ImageCorrespondences correspondences_; - Eigen::Isometry3d camera_T_target_; - Eigen::Matrix3d intrinsics_; -}; - -template -RegistrationCorrespondences::RegistrationCorrespondences( - const Eigen::Isometry3d& camera_T_target, const Eigen::Matrix3d& intrinsics, - const std::vector& target_t_target_points, const Eigen::VectorXd& distortion) - : camera_T_target_(camera_T_target), intrinsics_(intrinsics) { - for (const auto& target_t_target_point : target_t_target_points) { - const Eigen::Vector3d camera_t_target_point = camera_T_target_ * target_t_target_point; - if (camera_t_target_point.z() <= 0) continue; - const Eigen::Vector2d image_point = - Project3dPointToImageSpaceWithDistortion(camera_t_target_point, intrinsics_, distortion); - correspondences_.AddCorrespondence(image_point, target_t_target_point); - } -} - template std::vector RandomTargetMatchSets(const int num_match_sets, const int num_target_points_per_row_and_col, const Eigen::Matrix3d& intrinsics, const Eigen::VectorXd& distortion) { std::vector match_sets; match_sets.reserve(num_match_sets); for (int i = 0; i < num_match_sets; ++i) { - const auto correspondences = RegistrationCorrespondences( - RandomFrontFacingPose(), intrinsics, - TargetPoints(num_target_points_per_row_and_col, num_target_points_per_row_and_col), distortion); + const auto correspondences = vision_common::RegistrationCorrespondences( + vision_common::RandomFrontFacingPose(), intrinsics, + vision_common::TargetPoints(num_target_points_per_row_and_col, num_target_points_per_row_and_col), distortion); // Set inliers using correspondence point size since correspondence points with negative z are not // included in RegistrationCorrespondences std::vector inliers(correspondences.correspondences().size()); @@ -141,15 +68,15 @@ std::vector EvenlySpacedTargetMatchSets(const int num_pose_rows, const const Eigen::Matrix3d& intrinsics, const Eigen::VectorXd& distortion) { const std::vector target_poses = - EvenlySpacedTargetPoses(num_pose_rows, num_pose_cols, num_pose_y_levels); + vision_common::EvenlySpacedTargetPoses(num_pose_rows, num_pose_cols, num_pose_y_levels); std::vector match_sets; const int num_match_sets = target_poses.size(); match_sets.reserve(num_match_sets); const std::vector target_points = - TargetPoints(num_target_points_per_row_and_col, num_target_points_per_row_and_col); + vision_common::TargetPoints(num_target_points_per_row_and_col, num_target_points_per_row_and_col); for (int i = 0; i < num_match_sets; ++i) { const auto correspondences = - RegistrationCorrespondences(target_poses[i], intrinsics, target_points, distortion); + vision_common::RegistrationCorrespondences(target_poses[i], intrinsics, target_points, distortion); // Set inliers using correspondence point size since correspondence points with negative z are not // included in RegistrationCorrespondences std::vector inliers(correspondences.correspondences().size()); diff --git a/tools/calibration/tools/create_undistorted_images.cc b/tools/calibration/tools/create_undistorted_images.cc index f577994711..a70630e751 100644 --- a/tools/calibration/tools/create_undistorted_images.cc +++ b/tools/calibration/tools/create_undistorted_images.cc @@ -21,9 +21,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include #include #include @@ -31,7 +31,7 @@ #include namespace lc = localization_common; -namespace oc = optimization_common; +namespace vc = vision_common; namespace po = boost::program_options; namespace fs = boost::filesystem; @@ -114,13 +114,13 @@ int main(int argc, char** argv) { cv::Mat undistorted_image; // TODO(rsoussan): Avoid checking distortion param value/recreating distortion object each time if (distortion_type == "fov") { - oc::FovDistorter distorter; + vc::FovDistorter distorter; undistorted_image = distorter.Undistort(distorted_image, intrinsics, distortion_params); } else if (distortion_type == "rad") { - oc::RadDistorter distorter; + vc::RadDistorter distorter; undistorted_image = distorter.Undistort(distorted_image, intrinsics, distortion_params); } else if (distortion_type == "radtan") { - oc::RadTanDistorter distorter; + vc::RadTanDistorter distorter; undistorted_image = distorter.Undistort(distorted_image, intrinsics, distortion_params); } else { LogFatal("Invalid distortion type provided."); diff --git a/tools/calibration/tools/run_camera_target_based_intrinsics_calibrator.cc b/tools/calibration/tools/run_camera_target_based_intrinsics_calibrator.cc index eddb0e718a..af46925dd2 100644 --- a/tools/calibration/tools/run_camera_target_based_intrinsics_calibrator.cc +++ b/tools/calibration/tools/run_camera_target_based_intrinsics_calibrator.cc @@ -24,16 +24,16 @@ #include #include #include -#include -#include -#include +#include +#include +#include #include #include namespace ca = calibration; namespace lc = localization_common; -namespace oc = optimization_common; +namespace vc = vision_common; namespace po = boost::program_options; lc::ImageCorrespondences LoadTargetMatches(const std::string& match_file) { @@ -128,8 +128,9 @@ int main(int argc, char** argv) { std::string world; std::string output_file; po::options_description desc("Calibrates camera intrinsics using target detections."); - desc.add_options()("help", "produce help message")("corners-directory", po::value()->required(), - "Directory containing target detections to use for calibration.")( + desc.add_options()("help,h", "produce help message")( + "corners-directory", po::value()->required(), + "Directory containing target detections to use for calibration.")( "config-path,c", po::value()->required(), "Config path")( "robot-config-file,r", po::value(&robot_config_file)->default_value("config/robots/bumble.config"), "Robot config file")("world,w", po::value(&world)->default_value("iss"), "World name")( @@ -141,17 +142,16 @@ int main(int argc, char** argv) { po::variables_map vm; try { po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm); + if (vm.count("help") || (argc <= 1)) { + std::cout << desc << "\n"; + return 1; + } po::notify(vm); } catch (std::exception& e) { std::cerr << "Error: " << e.what() << "\n"; return 1; } - if (vm.count("help")) { - std::cout << desc << "\n"; - return 1; - } - const std::string corners_directory = vm["corners-directory"].as(); const std::string config_path = vm["config-path"].as(); @@ -180,11 +180,11 @@ int main(int argc, char** argv) { LoadAllTargetMatches(corners_directory, params.camera_target_based_intrinsics_calibrator.max_num_match_sets); LogInfo("Number of target match sets: " << target_matches.size()); if (params.distortion_type == "fov") { - Calibrate(params, target_matches, output_file); + Calibrate(params, target_matches, output_file); } else if (params.distortion_type == "rad") { - Calibrate(params, target_matches, output_file); + Calibrate(params, target_matches, output_file); } else if (params.distortion_type == "radtan") { - Calibrate(params, target_matches, output_file); + Calibrate(params, target_matches, output_file); } else { LogFatal("Invalid distortion type provided."); } diff --git a/tools/graph_bag/CMakeLists.txt b/tools/graph_bag/CMakeLists.txt index 827eb7bb1e..987d200a6d 100644 --- a/tools/graph_bag/CMakeLists.txt +++ b/tools/graph_bag/CMakeLists.txt @@ -27,24 +27,25 @@ if (USE_ROS) find_package(catkin2 REQUIRED COMPONENTS roscpp rosbag + camera + depth_odometry ff_util ff_msgs ff_common graph_localizer - camera - lk_optical_flow + imu_augmentor imu_bias_tester + lk_optical_flow localization_node - imu_augmentor ) # Find GTSAM find_package(GTSAM REQUIRED) catkin_package( - LIBRARIES ${PROJECT_NAME} + LIBRARIES ${PROJECT_NAME} ${OpenCV_LIBRARIES} INCLUDE_DIRS include - CATKIN_DEPENDS roscpp rosbag ff_util ff_msgs ff_common graph_localizer camera lk_optical_flow imu_bias_tester localization_node imu_augmentor + CATKIN_DEPENDS roscpp rosbag camera depth_odometry ff_util ff_msgs ff_common graph_localizer imu_augmentor imu_bias_tester lk_optical_flow localization_node ) ########### @@ -60,6 +61,7 @@ if (USE_ROS) # Declare C++ libraries add_library(${PROJECT_NAME} src/bag_imu_filterer.cc + src/depth_odometry_adder.cc src/graph_bag.cc src/graph_localizer_simulator.cc src/imu_bias_tester_adder.cc @@ -77,6 +79,12 @@ if (USE_ROS) target_link_libraries(run_bag_imu_filterer ${PROJECT_NAME} gflags gtsam ${catkin_LIBRARIES}) + ## Declare a C++ executable: run_bag_imu_filterer + add_executable(run_depth_odometry_adder tools/run_depth_odometry_adder.cc) + add_dependencies(run_depth_odometry_adder ${catkin_EXPORTED_TARGETS}) + target_link_libraries(run_depth_odometry_adder + ${PROJECT_NAME} ${catkin_LIBRARIES}) + ## Declare a C++ executable: run_graph_bag add_executable(run_graph_bag tools/run_graph_bag.cc) add_dependencies(run_graph_bag ${catkin_EXPORTED_TARGETS}) @@ -108,11 +116,13 @@ install(TARGETS ${PROJECT_NAME} # Install C++ executables install(TARGETS run_bag_imu_filterer DESTINATION bin) +install(TARGETS run_depth_odometry_adder DESTINATION bin) install(TARGETS run_graph_bag DESTINATION bin) install(TARGETS run_imu_bias_tester_adder DESTINATION bin) install(TARGETS run_sparse_mapping_pose_adder DESTINATION bin) install(CODE "execute_process( COMMAND ln -s ../../bin/run_bag_imu_filterer share/${PROJECT_NAME} + COMMAND ln -s ../../bin/run_depth_odometry_adder share/${PROJECT_NAME} COMMAND ln -s ../../bin/run_graph_bag share/${PROJECT_NAME} COMMAND ln -s ../../bin/run_imu_bias_tester_adder share/${PROJECT_NAME} COMMAND ln -s ../../bin/run_sparse_mapping_pose_adder share/${PROJECT_NAME} diff --git a/tools/graph_bag/include/graph_bag/depth_odometry_adder.h b/tools/graph_bag/include/graph_bag/depth_odometry_adder.h new file mode 100644 index 0000000000..91b7633448 --- /dev/null +++ b/tools/graph_bag/include/graph_bag/depth_odometry_adder.h @@ -0,0 +1,76 @@ +/* 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. + */ + +#ifndef GRAPH_BAG_DEPTH_ODOMETRY_ADDER_H_ +#define GRAPH_BAG_DEPTH_ODOMETRY_ADDER_H_ + +#include + +// TODO(rsoussan): There is a linking error on melodic for conflicting declarations of lz4 +// for flann serialization and ros serialization that occurs when this header is included. +// Once this is fixed (ros version > melodic) these defines and undefines can be removed. +#define LZ4_stream_t LZ4_stream_t_deprecated +#define LZ4_resetStream LZ4_resetStream_deprecated +#define LZ4_createStream LZ4_createStream_deprecated +#define LZ4_freeStream LZ4_freeStream_deprecated +#define LZ4_loadDict LZ4_loadDict_deprecated +#define LZ4_compress_fast_continue LZ4_compress_fast_continue_deprecated +#define LZ4_saveDict LZ4_saveDict_deprecated +#define LZ4_streamDecode_t LZ4_streamDecode_t_deprecated +#define LZ4_compress_continue LZ4_compress_continue_deprecated +#define LZ4_compress_limitedOutput_continue LZ4_compress_limitedOutput_continue_deprecated +#define LZ4_createStreamDecode LZ4_createStreamDecode_deprecated +#define LZ4_freeStreamDecode LZ4_freeStreamDecode_deprecated +#define LZ4_setStreamDecode LZ4_setStreamDecode_deprecated +#define LZ4_decompress_safe_continue LZ4_decompress_safe_continue_deprecated +#define LZ4_decompress_fast_continue LZ4_decompress_fast_continue_deprecated +#include +#undef LZ4_stream_t +#undef LZ4_resetStream +#undef LZ4_createStream +#undef LZ4_freeStream +#undef LZ4_loadDict +#undef LZ4_compress_fast_continue +#undef LZ4_saveDict +#undef LZ4_streamDecode_t +#undef LZ4_compress_continue +#undef LZ4_compress_limitedOutput_continue +#undef LZ4_createStreamDecode +#undef LZ4_freeStreamDecode +#undef LZ4_setStreamDecode +#undef LZ4_decompress_safe_continue +#undef LZ4_decompress_fast_continue + +#include + +namespace graph_bag { +class DepthOdometryAdder { + public: + DepthOdometryAdder(const std::string& input_bag_name, const std::string& output_bag_name, const bool save_all_topics, + const std::string& config_prefix = "localization/"); + void AddDepthOdometry(); + + private: + depth_odometry::DepthOdometryWrapper depth_odometry_wrapper_; + rosbag::Bag input_bag_; + rosbag::Bag output_bag_; + bool save_all_topics_; +}; +} // namespace graph_bag + +#endif // GRAPH_BAG_DEPTH_ODOMETRY_ADDER_H_ diff --git a/tools/graph_bag/include/graph_bag/graph_localizer_simulator.h b/tools/graph_bag/include/graph_bag/graph_localizer_simulator.h index aad5069597..e8284e3706 100644 --- a/tools/graph_bag/include/graph_bag/graph_localizer_simulator.h +++ b/tools/graph_bag/include/graph_bag/graph_localizer_simulator.h @@ -46,10 +46,13 @@ class GraphLocalizerSimulator : public graph_localizer::GraphLocalizerWrapper { void BufferFlightModeMsg(const ff_msgs::FlightMode& flight_mode_msg); + void BufferDepthOdometryMsg(const ff_msgs::DepthOdometry& depth_odometry_msg); + bool AddMeasurementsAndUpdateIfReady(const localization_common::Time& current_time); private: std::vector of_msg_buffer_; + std::vector depth_odometry_msg_buffer_; std::vector vl_msg_buffer_; std::vector ar_msg_buffer_; std::vector imu_msg_buffer_; diff --git a/tools/graph_bag/include/graph_bag/live_measurement_simulator.h b/tools/graph_bag/include/graph_bag/live_measurement_simulator.h index ee2271b286..398dc4c453 100644 --- a/tools/graph_bag/include/graph_bag/live_measurement_simulator.h +++ b/tools/graph_bag/include/graph_bag/live_measurement_simulator.h @@ -20,6 +20,7 @@ #define GRAPH_BAG_LIVE_MEASUREMENT_SIMULATOR_H_ #include +#include #include #include #include @@ -51,6 +52,7 @@ class LiveMeasurementSimulator { boost::optional GetImageMessage(const localization_common::Time current_time); boost::optional GetImuMessage(const localization_common::Time current_time); boost::optional GetOFMessage(const localization_common::Time current_time); + boost::optional GetDepthOdometryMessage(const localization_common::Time current_time); boost::optional GetVLMessage(const localization_common::Time current_time); boost::optional GetARMessage(const localization_common::Time current_time); boost::optional GetFlightModeMessage(const localization_common::Time current_time); @@ -70,6 +72,7 @@ class LiveMeasurementSimulator { boost::optional view_it_; std::map img_buffer_; MessageBuffer imu_buffer_; + MessageBuffer depth_odometry_buffer_; MessageBuffer flight_mode_buffer_; MessageBuffer of_buffer_; MessageBuffer vl_buffer_; diff --git a/tools/graph_bag/include/graph_bag/live_measurement_simulator_params.h b/tools/graph_bag/include/graph_bag/live_measurement_simulator_params.h index e687dcb99d..b98d667151 100644 --- a/tools/graph_bag/include/graph_bag/live_measurement_simulator_params.h +++ b/tools/graph_bag/include/graph_bag/live_measurement_simulator_params.h @@ -26,6 +26,7 @@ namespace graph_bag { struct LiveMeasurementSimulatorParams { MessageBufferParams imu; MessageBufferParams flight_mode; + MessageBufferParams depth_odometry; MessageBufferParams of; MessageBufferParams vl; MessageBufferParams ar; diff --git a/tools/graph_bag/include/graph_bag/utilities.h b/tools/graph_bag/include/graph_bag/utilities.h index a730d8e86a..6d04a13357 100644 --- a/tools/graph_bag/include/graph_bag/utilities.h +++ b/tools/graph_bag/include/graph_bag/utilities.h @@ -46,7 +46,7 @@ boost::optional CreateFeatureTrackImage( const sensor_msgs::ImageConstPtr& image_msg, const graph_localizer::FeatureTrackIdMap& feature_tracks, const camera::CameraParameters& camera_params, const std::vector& smart_factors = {}); -cv::Point Distort(const Eigen::Vector2d& undistorted_point, const camera::CameraParameters& params); +cv::Point2f Distort(const Eigen::Vector2d& undistorted_point, const camera::CameraParameters& params); std::vector SmartFactors(const graph_localizer::GraphLocalizer& graph); diff --git a/tools/graph_bag/package.xml b/tools/graph_bag/package.xml index 0ca1691995..76d9619b0f 100644 --- a/tools/graph_bag/package.xml +++ b/tools/graph_bag/package.xml @@ -17,26 +17,28 @@ eigen_conversions roscpp rosbag + camera + depth_odometry ff_util ff_msgs ff_common graph_localizer - camera - lk_optical_flow + imu_augmentor imu_bias_tester + lk_optical_flow localization_node - imu_augmentor eigen_conversions graph_localizer roscpp rosbag + camera + depth_odometry ff_util ff_msgs ff_common graph_localizer - camera - lk_optical_flow + imu_augmentor imu_bias_tester + lk_optical_flow localization_node - imu_augmentor diff --git a/tools/graph_bag/readme.md b/tools/graph_bag/readme.md index de2df0beab..6adfbce919 100644 --- a/tools/graph_bag/readme.md +++ b/tools/graph_bag/readme.md @@ -51,3 +51,8 @@ for further details and usage instructions. Creates groundtruth in parallel for a set of bagfiles. See 'rosrun graph\_bag groundtruth\_sweep.py -h' for further details and usage instructions. + +## depth\_odometry\_parameter\_sweep.py +Runs a parameter sweep for depth odometry on a bagfile and plots the results. +See 'rosrun depth\_odometry\_parameter\_sweep.py -h' +for further details and usage instructions. diff --git a/tools/graph_bag/scripts/average_results.py b/tools/graph_bag/scripts/average_results.py index 644cfcc6aa..5004841670 100755 --- a/tools/graph_bag/scripts/average_results.py +++ b/tools/graph_bag/scripts/average_results.py @@ -25,7 +25,7 @@ def combined_results(csv_files): - dataframes = [pd.read_csv(file) for file in csv_files] + dataframes = [pd.read_csv(file, header=None) for file in csv_files] if not dataframes: print("Failed to create dataframes") exit() @@ -58,7 +58,7 @@ def average_results(directory, csv_files): "directory", help="Full path to directory where results files are." ) args = parser.parse_args() - results_csv_files = utilities.get_files(args.directory, "*stats.csv") + results_csv_files = utilities.get_files_recursive(args.directory, "*stats.csv") if not results_csv_files: print("Failed to find stats.csv files") exit() diff --git a/tools/graph_bag/scripts/depth_odometry_parameter_sweep.py b/tools/graph_bag/scripts/depth_odometry_parameter_sweep.py new file mode 100755 index 0000000000..e9c9ceb258 --- /dev/null +++ b/tools/graph_bag/scripts/depth_odometry_parameter_sweep.py @@ -0,0 +1,239 @@ +#!/usr/bin/python +# +# 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. +""" +Runs a parameter sweep for depth odometry using the provided bagfile. +Parameters to sweep on are set in the make_value_ranges function. +All combinations of provided parameters are used for the sweep and the results +are plotted for various RMSEs. +""" + + +import argparse +import csv +import itertools +import math +import multiprocessing +import os + +import average_results +import config_creator +import numpy as np +import parameter_sweep_utilities +import plot_parameter_sweep_results +import utilities + + +# Run depth odometry with values. +# Add traceback so errors are forwarded, otherwise +# some errors are suppressed due to the multiprocessing +# library call +@utilities.full_traceback +def test_values( + values, + job_id, + value_names, + output_dir, + bag_file, + config_path, + robot_config, + world, + groundtruth_bagfile, + rmse_rel_start_time, + rmse_rel_end_time, +): + new_output_dir = os.path.join(output_dir, str(job_id)) + os.mkdir(new_output_dir) + depth_odometry_config_filepath = os.path.join( + config_path, "config", "localization/depth_odometry.config" + ) + new_depth_odometry_config_filepath = os.path.join( + new_output_dir, "depth_odometry.config" + ) + config_creator.make_config( + values, + value_names, + depth_odometry_config_filepath, + new_depth_odometry_config_filepath, + ) + output_bag = os.path.join(new_output_dir, "results.bag") + output_stats_file = os.path.join(new_output_dir, "depth_odom_stats.csv") + depth_odometry_config_prefix = new_output_dir + "/" + run_command = ( + "rosrun graph_bag run_depth_odometry_adder " + + bag_file + + " " + + config_path + + " -r " + + robot_config + + " -o " + + output_bag + + " -w " + + world + + " -p " + + depth_odometry_config_prefix + ) + os.system(run_command) + output_pdf_file = os.path.join(new_output_dir, str(job_id) + "_output.pdf") + output_csv_file = os.path.join(new_output_dir, "depth_odom_stats.csv") + plot_command = ( + "rosrun graph_bag plot_results_main.py " + + output_bag + + " --output-file " + + output_pdf_file + + " --output-csv-file " + + output_csv_file + + " -g " + + groundtruth_bagfile + + " --rmse-rel-start-time " + + str(rmse_rel_start_time) + + " --rmse-rel-end-time " + + str(rmse_rel_end_time) + ) + os.system(plot_command) + + +# Helper that unpacks arguments and calls original function +# Aides running jobs in parallel as pool only supports +# passing a single argument to workers +def test_values_helper(zipped_vals): + return test_values(*zipped_vals) + + +def parameter_sweep( + all_value_combos, + value_names, + output_dir, + bag_file, + config_path, + robot_config, + world, + groundtruth_bagfile, + rmse_rel_start_time=0, + rmse_rel_end_time=-1, +): + job_ids = list(range(len(all_value_combos))) + num_processes = 15 + pool = multiprocessing.Pool(num_processes) + # izip arguments so we can pass as one argument to pool worker + pool.map( + test_values_helper, + list( + zip( + all_value_combos, + job_ids, + itertools.repeat(value_names), + itertools.repeat(output_dir), + itertools.repeat(bag_file), + itertools.repeat(config_path), + itertools.repeat(robot_config), + itertools.repeat(world), + itertools.repeat(groundtruth_bagfile), + itertools.repeat(rmse_rel_start_time), + itertools.repeat(rmse_rel_end_time), + ) + ), + ) + parameter_sweep_utilities.concat_results( + job_ids, output_dir, "depth_odom_stats.csv" + ) + + +def make_value_ranges(): + value_ranges = [] + value_names = [] + steps = 10 + + value_ranges.append(np.linspace(10, 100, steps, endpoint=True)) + value_names.append("lk_max_corners") + return value_ranges, value_names + + +def make_values_and_parameter_sweep( + output_dir, + bag_file, + config_path, + robot_config, + world, + groundtruth_bagfile, + rmse_rel_start_time=0, + rmse_rel_end_time=-1, +): + output_dir = utilities.create_directory(output_dir) + print(("Output directory for results is {}".format(output_dir))) + + value_ranges, value_names = make_value_ranges() + parameter_sweep_utilities.save_values( + value_names, value_ranges, "individual_value_ranges.csv", output_dir + ) + + all_value_combos = parameter_sweep_utilities.make_all_value_combinations( + value_ranges + ) + parameter_sweep_utilities.save_values( + value_names, all_value_combos, "all_value_combos.csv", output_dir + ) + + parameter_sweep( + all_value_combos, + value_names, + output_dir, + bag_file, + config_path, + robot_config, + world, + groundtruth_bagfile, + rmse_rel_start_time, + rmse_rel_end_time, + ) + combined_results_file = os.path.join(output_dir, "param_sweep_combined_results.csv") + value_combos_file = os.path.join(output_dir, "all_value_combos.csv") + results_pdf_file = os.path.join(output_dir, "param_sweep_results.pdf") + plot_parameter_sweep_results.create_plots( + results_pdf_file, combined_results_file, value_combos_file + ) + return output_dir + + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter + ) + parser.add_argument("bag_file", help="Full path to bagfile.") + parser.add_argument("config_path", help="Full path to config path.") + parser.add_argument("robot_config", help="Relative path to robot config.") + parser.add_argument("world", help="World being used.") + parser.add_argument("-g", "--groundtruth-bagfile", default=None) + parser.add_argument( + "--directory", + default=None, + help="Full path to output directory where files will be saved. If not specified, timestamped directory will be created in current path.", + ) + args = parser.parse_args() + # Default set groundtruth bagfile to normal bagfile + if not args.groundtruth_bagfile: + args.groundtruth_bagfile = args.bag_file + + make_values_and_parameter_sweep( + args.directory, + args.bag_file, + args.config_path, + args.robot_config, + args.world, + args.groundtruth_bagfile, + ) diff --git a/tools/graph_bag/scripts/get_msg_stats.py b/tools/graph_bag/scripts/get_msg_stats.py new file mode 100755 index 0000000000..eb05ec6266 --- /dev/null +++ b/tools/graph_bag/scripts/get_msg_stats.py @@ -0,0 +1,74 @@ +#!/usr/bin/env python +# 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. +""" +Generates mean, min, max, and stddev stats for provided topic and its field +in a bagfile. +""" + +import argparse +import os +import sys + +import numpy as np +import rosbag + + +def get_stats(bagfile, topic, field, use_size=False): + with rosbag.Bag(bagfile, "r") as bag: + vals = [] + for _, msg, _ in bag.read_messages([topic]): + val = getattr(msg, field) + if use_size: + vals.append(len(val)) + else: + vals.append(val) + + mean_val = np.mean(vals) + min_val = np.min(vals) + max_val = np.max(vals) + stddev_val = np.std(vals) + print(("Mean " + field + ": " + str(mean_val))) + print(("Min " + field + ": " + str(min_val))) + print(("Max " + field + ": " + str(max_val))) + print(("Stddev " + field + ": " + str(stddev_val))) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter + ) + parser.add_argument("bagfile", help="Input bagfile containing desired topic.") + parser.add_argument("topic", help="Topic in the bagfile to get stats for.") + parser.add_argument( + "field", + help="Field of the message published on the provided topic to get stats for.", + ) + parser.add_argument( + "--use-size", + "-s", + default=False, + action="store_true", + help="Get stats for the size of the provided field.", + ) + + args = parser.parse_args() + if not os.path.isfile(args.bagfile): + print(("Bag file " + args.bagfile + " does not exist.")) + sys.exit() + + get_stats(args.bagfile, args.topic, args.field, args.use_size) diff --git a/tools/graph_bag/scripts/loc_states.py b/tools/graph_bag/scripts/loc_states.py index 694cb60866..1623a5ad55 100644 --- a/tools/graph_bag/scripts/loc_states.py +++ b/tools/graph_bag/scripts/loc_states.py @@ -40,7 +40,7 @@ def __init__(self, loc_type, topic): self.gyro_bias_covariances = vector3ds.Vector3ds() def add_loc_state(self, msg, bag_start_time=0): - self.add_pose(msg.pose, msg.header.stamp, bag_start_time) + self.add_pose_msg(msg.pose, msg.header.stamp, bag_start_time) if hasattr(msg, "num_detected_of_features"): self.num_detected_of_features.append(msg.num_detected_of_features) if hasattr(msg, "num_detected_ml_features"): diff --git a/tools/graph_bag/scripts/parameter_sweep.py b/tools/graph_bag/scripts/parameter_sweep.py index c811e5d148..69f7ba69f8 100755 --- a/tools/graph_bag/scripts/parameter_sweep.py +++ b/tools/graph_bag/scripts/parameter_sweep.py @@ -27,6 +27,7 @@ import average_results import config_creator import numpy as np +import parameter_sweep_utilities import plot_parameter_sweep_results import utilities @@ -113,18 +114,6 @@ def test_values_helper(zipped_vals): return test_values(*zipped_vals) -def concat_results(job_ids, directory): - results_csv_files = [] - for job_id in job_ids: - results_csv_files.append( - os.path.join(directory, str(job_id), "graph_stats.csv") - ) - # Results are written in job id order - combined_results = average_results.combined_results(results_csv_files) - combined_results_file = os.path.join(directory, "param_sweep_combined_results.csv") - combined_results.to_csv(combined_results_file, index=False) - - def parameter_sweep( all_value_combos, value_names, @@ -165,11 +154,7 @@ def parameter_sweep( ) ), ) - concat_results(job_ids, output_dir) - - -def make_all_value_combinations(value_ranges): - return list(itertools.product(*value_ranges)) + parameter_sweep_utilities.concat_results(job_ids, output_dir, "graph_stats.csv") def make_value_ranges(): @@ -193,13 +178,6 @@ def make_value_ranges(): return value_ranges, value_names -def save_values(value_names, values, filename, output_dir): - with open(os.path.join(output_dir, filename), "w") as values_file: - writer = csv.writer(values_file) - writer.writerow(value_names) - writer.writerows(values) - - def make_values_and_parameter_sweep( output_dir, bag_file, @@ -217,10 +195,16 @@ def make_values_and_parameter_sweep( print(("Output directory for results is {}".format(output_dir))) value_ranges, value_names = make_value_ranges() - save_values(value_names, value_ranges, "individual_value_ranges.csv", output_dir) + parameter_sweep_utilities.save_values( + value_names, value_ranges, "individual_value_ranges.csv", output_dir + ) - all_value_combos = make_all_value_combinations(value_ranges) - save_values(value_names, all_value_combos, "all_value_combos.csv", output_dir) + all_value_combos = parameter_sweep_utilities.make_all_value_combinations( + value_ranges + ) + parameter_sweep_utilities.save_values( + value_names, all_value_combos, "all_value_combos.csv", output_dir + ) parameter_sweep( all_value_combos, diff --git a/tools/graph_bag/scripts/parameter_sweep_utilities.py b/tools/graph_bag/scripts/parameter_sweep_utilities.py new file mode 100755 index 0000000000..5e71fee0fa --- /dev/null +++ b/tools/graph_bag/scripts/parameter_sweep_utilities.py @@ -0,0 +1,45 @@ +#!/usr/bin/python +# +# 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. + +import csv +import itertools +import os + +import average_results + + +def concat_results(job_ids, directory, stats_filename): + results_csv_files = [] + for job_id in job_ids: + results_csv_files.append(os.path.join(directory, str(job_id), stats_filename)) + # Results are written in job id order + combined_results = average_results.combined_results(results_csv_files) + combined_results_file = os.path.join(directory, "param_sweep_combined_results.csv") + combined_results.to_csv(combined_results_file, index=False) + + +def make_all_value_combinations(value_ranges): + return list(itertools.product(*value_ranges)) + + +def save_values(value_names, values, filename, output_dir): + with open(os.path.join(output_dir, filename), "w") as values_file: + writer = csv.writer(values_file) + writer.writerow(value_names) + writer.writerows(values) diff --git a/tools/graph_bag/scripts/plot_parameter_sweep_results.py b/tools/graph_bag/scripts/plot_parameter_sweep_results.py index a57f425134..5b334312c6 100755 --- a/tools/graph_bag/scripts/plot_parameter_sweep_results.py +++ b/tools/graph_bag/scripts/plot_parameter_sweep_results.py @@ -33,7 +33,10 @@ def create_plot(pdf, csv_file, value_combos_file, prefix=""): dataframe = pd.read_csv(csv_file) - rmses = dataframe[prefix + "rmse"] + try: + rmses = dataframe[prefix + "rmse"] + except: + return x_axis_vals = [] x_axis_label = "" if value_combos_file: @@ -97,6 +100,7 @@ def create_plot(pdf, csv_file, value_combos_file, prefix=""): plt.close() +# TODO(rsoussan): Make this more generic, don't hardcode each RMSE type def create_plots(output_file, csv_file, value_combos_file): with PdfPages(output_file) as pdf: # Graph RMSEs @@ -122,6 +126,12 @@ def create_plots(output_file, csv_file, value_combos_file): pdf, csv_file, value_combos_file, "rel_imu_bias_tester_orientation_" ) + # Depth Odometry RMSEs + create_plot(pdf, csv_file, value_combos_file, "depth_odometry_") + create_plot(pdf, csv_file, value_combos_file, "depth_odometry_orientation_") + create_plot(pdf, csv_file, value_combos_file, "rel_depth_odometry_") + create_plot(pdf, csv_file, value_combos_file, "rel_depth_odometry_orientation_") + if __name__ == "__main__": parser = argparse.ArgumentParser() diff --git a/tools/graph_bag/scripts/plot_results.py b/tools/graph_bag/scripts/plot_results.py index 4774a9d09c..0f80ce5afa 100644 --- a/tools/graph_bag/scripts/plot_results.py +++ b/tools/graph_bag/scripts/plot_results.py @@ -139,10 +139,25 @@ def add_graph_plots( plt.close() # Integrated Velocities + integrated_graph_localization_states = utilities.integrate_velocities( + graph_localization_states + ) + plot_positions( + pdf, integrated_graph_localization_states, sparse_mapping_poses, ar_tag_poses + ) + + +# TODO(rsoussan): Use this for other pose plotting in this script +def plot_poses(pdf, poses, sparse_mapping_poses, ar_tag_poses): + plot_positions(pdf, poses, sparse_mapping_poses, ar_tag_poses) + plot_orientations(pdf, poses, sparse_mapping_poses, ar_tag_poses) + + +def plot_positions(pdf, position_poses, sparse_mapping_poses, ar_tag_poses): position_plotter = vector3d_plotter.Vector3dPlotter( "Time (s)", "Position (m)", - "Integrated Graph Velocities vs. Sparse Mapping Position", + position_poses.pose_type + " vs. Sparse Mapping Position", True, ) position_plotter.add_pose_position( @@ -160,12 +175,46 @@ def add_graph_plots( markeredgewidth=0.1, markersize=1.5, ) + position_plotter.add_pose_position(position_poses) + position_plotter.plot(pdf) - integrated_graph_localization_states = utilities.integrate_velocities( - graph_localization_states + +def plot_orientations(pdf, poses, sparse_mapping_poses, ar_tag_poses): + orientation_plotter = vector3d_plotter.Vector3dPlotter( + "Time (s)", + "Orientation (deg)", + poses.pose_type + " vs. Sparse Mapping Orientation", + True, ) - position_plotter.add_pose_position(integrated_graph_localization_states) - position_plotter.plot(pdf) + orientation_plotter.add_pose_orientation( + sparse_mapping_poses, + linestyle="None", + marker="o", + markeredgewidth=0.1, + markersize=1.5, + ) + if ar_tag_poses.times: + orientation_plotter.add_pose_orientation( + ar_tag_poses, + linestyle="None", + marker="x", + markeredgewidth=0.1, + markersize=1.5, + ) + orientation_plotter.add_pose_orientation(poses) + orientation_plotter.plot(pdf) + + +def plot_covariances(pdf, times, covariances, name): + plt.figure() + title = name + " Covariance" + plt.plot(times, l2_map(covariances), "r", linewidth=0.5, label=title) + plt.title(title) + plt.xlabel("Time (s)") + plt.ylabel(title) + plt.legend(prop={"size": 6}) + pdf.savefig() + plt.close() def plot_features( @@ -700,6 +749,30 @@ def load_pose_msgs(vec_of_poses, bag, bag_start_time): break +def load_odometry_msgs(vec_of_poses, bag, bag_start_time): + topics = [poses.topic for poses in vec_of_poses] + for topic, msg, t in bag.read_messages(topics): + for poses in vec_of_poses: + if poses.topic == topic: + poses.add_msg_with_covariance( + msg.odometry.body_F_source_T_target, + msg.header.stamp, + bag_start_time, + ) + break + + +def load_pose_with_cov_msgs(vec_of_poses, bag, bag_start_time): + topics = [poses.topic for poses in vec_of_poses] + for topic, msg, t in bag.read_messages(topics): + for poses in vec_of_poses: + if poses.topic == topic: + poses.add_msg_with_covariance( + msg.pose, msg.header.stamp, bag_start_time + ) + break + + def load_loc_state_msgs(vec_of_loc_states, bag, bag_start_time): topics = [loc_states.topic for loc_states in vec_of_loc_states] for topic, msg, t in bag.read_messages(topics): @@ -740,6 +813,9 @@ def create_plots( imu_bias_tester_poses = poses.Poses("Imu Bias Tester", "/imu_bias_tester/pose") vec_of_poses = [ar_tag_poses, imu_bias_tester_poses] load_pose_msgs(vec_of_poses, bag, bag_start_time) + has_depth_odom = has_topic(bag, "/loc/depth/odom") + depth_odom_relative_poses = poses.Poses("Depth Odom", "/loc/depth/odom") + load_odometry_msgs([depth_odom_relative_poses], bag, bag_start_time) groundtruth_vec_of_poses = [sparse_mapping_poses] load_pose_msgs(groundtruth_vec_of_poses, groundtruth_bag, bag_start_time) @@ -785,6 +861,37 @@ def create_plots( add_other_loc_plots( pdf, graph_localization_states, graph_localization_states ) + if has_depth_odom: + depth_odom_poses = utilities.make_absolute_poses_from_relative_poses( + sparse_mapping_poses, depth_odom_relative_poses, "Depth Odometry" + ) + plot_poses(pdf, depth_odom_poses, sparse_mapping_poses, ar_tag_poses) + # Note that for absolute time difference tolerance depth images and groudtruth use different sensor data + # and therefore have less similar timestamps. This timestamp difference reduces the accuracy of depth odometry + # groundtruth comparison. + plot_loc_state_stats( + pdf, + depth_odom_poses, + sparse_mapping_poses, + output_csv_file, + "depth_odometry_", + 0.01, + False, + rmse_rel_start_time=rmse_rel_start_time, + rmse_rel_end_time=rmse_rel_end_time, + ) + plot_covariances( + pdf, + depth_odom_relative_poses.times, + depth_odom_relative_poses.covariances.position, + "Depth Odometry Position", + ) + plot_covariances( + pdf, + depth_odom_relative_poses.times, + depth_odom_relative_poses.covariances.orientation, + "Depth Odometry Orientation", + ) plot_loc_state_stats( pdf, graph_localization_states, diff --git a/tools/graph_bag/scripts/pose.py b/tools/graph_bag/scripts/pose.py new file mode 100644 index 0000000000..2fe84b49d3 --- /dev/null +++ b/tools/graph_bag/scripts/pose.py @@ -0,0 +1,29 @@ +#!/usr/bin/python +# +# 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. + + +class Pose: + def __init__(self, orientation, position): + self.orientation = orientation + self.position = position + + def __mul__(self, pose_b): + new_orientation = self.orientation * pose_b.orientation + new_position = self.orientation.apply(pose_b.position) + self.position + return Pose(new_orientation, new_position) diff --git a/tools/graph_bag/scripts/pose_covariances.py b/tools/graph_bag/scripts/pose_covariances.py new file mode 100644 index 0000000000..43f3388030 --- /dev/null +++ b/tools/graph_bag/scripts/pose_covariances.py @@ -0,0 +1,52 @@ +#!/usr/bin/python +# +# 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. + +import vector3ds + + +class PoseCovariances: + def __init__(self): + self.position = vector3ds.Vector3ds() + self.orientation = vector3ds.Vector3ds() + + # Assumes 6x6 covariance matrix stored in row major order + # 0 1 2 3 4 5 + # 6 7 8 9 10 11 + # 12 13 14 15 16 17 + # 18 19 20 21 22 23 + # 24 25 26 27 28 29 + # 30 31 32 33 34 35 + # Then the position submatrix is + # 0 1 2 + # 6 7 8 + # 12 13 14 + # And the orientation submatrix is + # 21 22 23 + # 27 28 29 + # 33 34 35 + def add(self, covariance_vector): + x = covariance_vector[0] + y = covariance_vector[7] + z = covariance_vector[14] + self.position.add(x, y, z) + + rot_x = covariance_vector[21] + rot_y = covariance_vector[28] + rot_z = covariance_vector[35] + self.orientation.add(rot_x, rot_y, rot_z) diff --git a/tools/graph_bag/scripts/poses.py b/tools/graph_bag/scripts/poses.py index 0dafd970a9..e1321086d7 100644 --- a/tools/graph_bag/scripts/poses.py +++ b/tools/graph_bag/scripts/poses.py @@ -17,7 +17,10 @@ # License for the specific language governing permissions and limitations # under the License. +import numpy as np import orientations +import pose +import pose_covariances import scipy.spatial.transform import vector3ds @@ -26,27 +29,49 @@ class Poses(object): def __init__(self, pose_type, topic): self.positions = vector3ds.Vector3ds() self.orientations = orientations.Orientations() + self.covariances = pose_covariances.PoseCovariances() self.times = [] self.pose_type = pose_type self.topic = topic - def add_pose(self, pose_msg, timestamp, bag_start_time=0): - self.positions.add( - pose_msg.position.x, pose_msg.position.y, pose_msg.position.z + def init_from_poses(self, poses_list, times): + for pose in poses_list: + self.add_pose(pose) + self.times = times + + def add_pose(self, pose): + self.add_orientation_and_position( + pose.orientation, pose.position[0], pose.position[1], pose.position[2] ) - euler_angles = scipy.spatial.transform.Rotation.from_quat( + + def add_orientation_and_position(self, orientation, x, y, z): + self.positions.add(x, y, z) + euler_angles = orientation.as_euler("ZYX", degrees=True) + self.orientations.add(euler_angles[0], euler_angles[1], euler_angles[2]) + + def add_covariance_msg(self, covariance): + self.covariances.add(covariance) + + def add_pose_msg(self, pose_msg, timestamp, bag_start_time=0): + orientation = scipy.spatial.transform.Rotation.from_quat( [ pose_msg.orientation.x, pose_msg.orientation.y, pose_msg.orientation.z, pose_msg.orientation.w, ] - ).as_euler("ZYX", degrees=True) - self.orientations.add(euler_angles[0], euler_angles[1], euler_angles[2]) + ) + self.add_orientation_and_position( + orientation, pose_msg.position.x, pose_msg.position.y, pose_msg.position.z + ) self.times.append(timestamp.secs + 1e-9 * timestamp.nsecs - bag_start_time) + def add_msg_with_covariance(self, msg, timestamp, bag_start_time=0): + self.add_pose_msg(msg.pose, timestamp, bag_start_time) + self.add_covariance_msg(msg.covariance) + def add_msg(self, msg, timestamp, bag_start_time=0): - self.add_pose(msg.pose, timestamp, bag_start_time) + self.add_pose_msg(msg.pose, timestamp, bag_start_time) def position_vector(self, index): return [ @@ -54,3 +79,8 @@ def position_vector(self, index): self.positions.ys[index], self.positions.zs[index], ] + + def pose(self, index): + return pose.Pose( + self.orientations.get_rotation(index), np.array(self.position_vector(index)) + ) diff --git a/tools/graph_bag/scripts/utilities.py b/tools/graph_bag/scripts/utilities.py index 47b15d56dc..e434db4100 100644 --- a/tools/graph_bag/scripts/utilities.py +++ b/tools/graph_bag/scripts/utilities.py @@ -22,6 +22,7 @@ import numpy as np import pandas as pd +import pose import poses import rosbag @@ -149,6 +150,24 @@ def get_topic_rates( print(("Stddev time diff: " + str(stddev_time_diff))) +def make_absolute_poses_from_relative_poses(absolute_poses, relative_poses, name): + starting_relative_time = relative_poses.times[0] + np_times = np.array(absolute_poses.times) + closest_index = np.argmin(np.abs(np_times - starting_relative_time)) + start_pose = absolute_poses.pose(closest_index) + new_pose = start_pose + new_poses_list = [start_pose] + new_poses_times = [absolute_poses.times[closest_index]] + for index in range(len(relative_poses.times)): + relative_pose = relative_poses.pose(index) + new_pose = new_pose * relative_pose + new_poses_list.append(new_pose) + new_poses_times.append(relative_poses.times[index]) + new_poses = poses.Poses(name, "") + new_poses.init_from_poses(new_poses_list, new_poses_times) + return new_poses + + def integrate_velocities(localization_states): delta_times = [ j - i @@ -156,46 +175,69 @@ def integrate_velocities(localization_states): ] # Make sure times are same length as velocities, ignore last velocity delta_times.append(0) - integrated_positions = poses.Poses("Integrated Graph Velocities", "") # TODO(rsoussan): Integrate angular velocities? # TODO(rsoussan): central difference instead? x_increments = [ velocity * delta_t for velocity, delta_t in zip(localization_states.velocities.xs, delta_times) ] - cumulative_x_increments = np.cumsum(x_increments) - integrated_positions.positions.xs = [ - localization_states.positions.xs[0] + cumulative_x_increment - for cumulative_x_increment in cumulative_x_increments - ] y_increments = [ velocity * delta_t for velocity, delta_t in zip(localization_states.velocities.ys, delta_times) ] - cumulative_y_increments = np.cumsum(y_increments) - integrated_positions.positions.ys = [ - localization_states.positions.ys[0] + cumulative_y_increment - for cumulative_y_increment in cumulative_y_increments - ] z_increments = [ velocity * delta_t for velocity, delta_t in zip(localization_states.velocities.zs, delta_times) ] + + return add_increments_to_absolute_pose( + x_increments, + y_increments, + z_increments, + localization_states.positions.xs[0], + localization_states.positions.ys[0], + localization_states.positions.zs[0], + localization_states.times, + "Integrated Graph Velocities", + ) + + +def add_increments_to_absolute_pose( + x_increments, + y_increments, + z_increments, + starting_x, + starting_y, + starting_z, + times, + poses_name="Increment Poses", +): + integrated_positions = poses.Poses(poses_name, "") + cumulative_x_increments = np.cumsum(x_increments) + integrated_positions.positions.xs = [ + starting_x + cumulative_x_increment + for cumulative_x_increment in cumulative_x_increments + ] + cumulative_y_increments = np.cumsum(y_increments) + integrated_positions.positions.ys = [ + starting_y + cumulative_y_increment + for cumulative_y_increment in cumulative_y_increments + ] cumulative_z_increments = np.cumsum(z_increments) integrated_positions.positions.zs = [ - localization_states.positions.zs[0] + cumulative_z_increment + starting_z + cumulative_z_increment for cumulative_z_increment in cumulative_z_increments ] # Add start positions - integrated_positions.positions.xs.insert(0, localization_states.positions.xs[0]) - integrated_positions.positions.ys.insert(0, localization_states.positions.ys[0]) - integrated_positions.positions.zs.insert(0, localization_states.positions.zs[0]) + integrated_positions.positions.xs.insert(0, starting_x) + integrated_positions.positions.ys.insert(0, starting_y) + integrated_positions.positions.zs.insert(0, starting_z) # Remove last elements (no timestamp for these) del integrated_positions.positions.xs[-1] del integrated_positions.positions.ys[-1] del integrated_positions.positions.zs[-1] - integrated_positions.times = localization_states.times + integrated_positions.times = times return integrated_positions diff --git a/tools/graph_bag/src/depth_odometry_adder.cc b/tools/graph_bag/src/depth_odometry_adder.cc new file mode 100644 index 0000000000..7e05925eab --- /dev/null +++ b/tools/graph_bag/src/depth_odometry_adder.cc @@ -0,0 +1,85 @@ +/* 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. + */ + +#include +#include +#include +#include + +#include + +namespace graph_bag { +namespace lc = localization_common; +DepthOdometryAdder::DepthOdometryAdder(const std::string& input_bag_name, const std::string& output_bag_name, + const bool save_all_topics, const std::string& config_prefix) + : input_bag_(input_bag_name, rosbag::bagmode::Read), + output_bag_(output_bag_name, rosbag::bagmode::Write), + save_all_topics_(save_all_topics), + depth_odometry_wrapper_(config_prefix) {} + +void DepthOdometryAdder::AddDepthOdometry() { + const std::string point_cloud_topic = static_cast(TOPIC_HARDWARE_PICOFLEXX_PREFIX) + + static_cast(TOPIC_HARDWARE_NAME_HAZ_CAM) + + static_cast(TOPIC_HARDWARE_PICOFLEXX_SUFFIX); + const std::string image_topic = static_cast(TOPIC_HARDWARE_PICOFLEXX_PREFIX) + + static_cast(TOPIC_HARDWARE_NAME_HAZ_CAM) + + static_cast(TOPIC_HARDWARE_PICOFLEXX_SUFFIX_EXTENDED) + + static_cast(TOPIC_HARDWARE_PICOFLEXX_SUFFIX_AMPLITUDE_IMAGE); + // Other localization specific topics + std::vector topics; + topics.push_back(std::string("/") + point_cloud_topic); + topics.push_back(std::string("/") + image_topic); + topics.push_back(std::string("/") + TOPIC_SPARSE_MAPPING_POSE); + topics.push_back("/tf"); + topics.push_back("/tf_static"); + topics.push_back("/loc/ml/features"); + topics.push_back("/loc/of/features"); + topics.push_back("/loc/ar/features"); + topics.push_back("/hw/imu"); + topics.push_back("/mob/flight_mode"); + topics.push_back("/gnc/ekf"); + topics.push_back("/graph_loc/state"); + + // Use unique ptr since ternary initialization fails as view construction from another view is private + std::unique_ptr view; + if (save_all_topics_) + view.reset(new rosbag::View(input_bag_)); + else + view.reset(new rosbag::View(input_bag_, rosbag::TopicQuery(topics))); + for (const rosbag::MessageInstance msg : *view) { + if (string_ends_with(msg.getTopic(), point_cloud_topic)) { + const sensor_msgs::PointCloud2ConstPtr& point_cloud_msg = msg.instantiate(); + const auto pose_msgs = depth_odometry_wrapper_.PointCloudCallback(point_cloud_msg); + for (const auto& pose_msg : pose_msgs) { + const ros::Time timestamp = lc::RosTimeFromHeader(point_cloud_msg->header); + output_bag_.write(std::string("/") + TOPIC_LOCALIZATION_DEPTH_ODOM, timestamp, pose_msg); + } + if (!save_all_topics_) continue; + } else if (string_ends_with(msg.getTopic(), image_topic)) { + const sensor_msgs::ImageConstPtr& image_msg = msg.instantiate(); + const auto pose_msgs = depth_odometry_wrapper_.ImageCallback(image_msg); + for (const auto& pose_msg : pose_msgs) { + const ros::Time timestamp = lc::RosTimeFromHeader(image_msg->header); + output_bag_.write(std::string("/") + TOPIC_LOCALIZATION_DEPTH_ODOM, timestamp, pose_msg); + } + if (!save_all_topics_) continue; + } + output_bag_.write(msg.getTopic(), msg.getTime(), msg); + } +} +} // namespace graph_bag diff --git a/tools/graph_bag/src/graph_bag.cc b/tools/graph_bag/src/graph_bag.cc index 9f64496018..a719355ac2 100644 --- a/tools/graph_bag/src/graph_bag.cc +++ b/tools/graph_bag/src/graph_bag.cc @@ -112,6 +112,10 @@ void GraphBag::Run() { SaveMsg(*imu_augmented_loc_msg, TOPIC_GNC_EKF, results_bag_); } } + const auto depth_odometry_msg = live_measurement_simulator_->GetDepthOdometryMessage(current_time); + if (depth_odometry_msg) { + graph_localizer_simulator_->BufferDepthOdometryMsg(*depth_odometry_msg); + } const auto of_msg = live_measurement_simulator_->GetOFMessage(current_time); if (of_msg) { graph_localizer_simulator_->BufferOpticalFlowMsg(*of_msg); diff --git a/tools/graph_bag/src/graph_localizer_simulator.cc b/tools/graph_bag/src/graph_localizer_simulator.cc index 668c87e7df..b035a6f8d4 100644 --- a/tools/graph_bag/src/graph_localizer_simulator.cc +++ b/tools/graph_bag/src/graph_localizer_simulator.cc @@ -28,6 +28,10 @@ void GraphLocalizerSimulator::BufferOpticalFlowMsg(const ff_msgs::Feature2dArray of_msg_buffer_.emplace_back(feature_array_msg); } +void GraphLocalizerSimulator::BufferDepthOdometryMsg(const ff_msgs::DepthOdometry& depth_odometry_msg) { + depth_odometry_msg_buffer_.emplace_back(depth_odometry_msg); +} + void GraphLocalizerSimulator::BufferVLVisualLandmarksMsg(const ff_msgs::VisualLandmarks& visual_landmarks_msg) { vl_msg_buffer_.emplace_back(visual_landmarks_msg); } @@ -61,7 +65,10 @@ bool GraphLocalizerSimulator::AddMeasurementsAndUpdateIfReady(const lc::Time& cu ImuCallback(imu_msg); } imu_msg_buffer_.clear(); - + for (const auto& depth_odometry_msg : depth_odometry_msg_buffer_) { + DepthOdometryCallback(depth_odometry_msg); + } + depth_odometry_msg_buffer_.clear(); for (const auto& of_msg : of_msg_buffer_) { OpticalFlowCallback(of_msg); } diff --git a/tools/graph_bag/src/live_measurement_simulator.cc b/tools/graph_bag/src/live_measurement_simulator.cc index fc00a7f18a..47bbf2dc20 100644 --- a/tools/graph_bag/src/live_measurement_simulator.cc +++ b/tools/graph_bag/src/live_measurement_simulator.cc @@ -34,6 +34,7 @@ LiveMeasurementSimulator::LiveMeasurementSimulator(const LiveMeasurementSimulato kImageTopic_(params.image_topic), imu_buffer_(params.imu), flight_mode_buffer_(params.flight_mode), + depth_odometry_buffer_(params.depth_odometry), of_buffer_(params.of), vl_buffer_(params.vl), ar_buffer_(params.ar) { @@ -65,6 +66,9 @@ LiveMeasurementSimulator::LiveMeasurementSimulator(const LiveMeasurementSimulato topics.push_back(std::string("/") + TOPIC_LOCALIZATION_AR_FEATURES); topics.push_back(TOPIC_LOCALIZATION_AR_FEATURES); + topics.push_back(std::string("/") + TOPIC_LOCALIZATION_DEPTH_ODOM); + topics.push_back(TOPIC_LOCALIZATION_DEPTH_ODOM); + topics.push_back(std::string("/") + TOPIC_MOBILITY_FLIGHT_MODE); topics.push_back(TOPIC_MOBILITY_FLIGHT_MODE); @@ -107,6 +111,9 @@ bool LiveMeasurementSimulator::ProcessMessage() { } else if (string_ends_with(msg.getTopic(), TOPIC_MOBILITY_FLIGHT_MODE)) { const ff_msgs::FlightModeConstPtr flight_mode = msg.instantiate(); flight_mode_buffer_.BufferMessage(*flight_mode); + } else if (string_ends_with(msg.getTopic(), TOPIC_LOCALIZATION_DEPTH_ODOM)) { + const ff_msgs::DepthOdometryConstPtr depth_odometry = msg.instantiate(); + depth_odometry_buffer_.BufferMessage(*depth_odometry); } else if (string_ends_with(msg.getTopic(), TOPIC_LOCALIZATION_AR_FEATURES)) { // Always use ar features until have data with dock cam images const ff_msgs::VisualLandmarksConstPtr ar_features = msg.instantiate(); @@ -146,6 +153,9 @@ boost::optional LiveMeasurementSimulator::GetFlightModeMess boost::optional LiveMeasurementSimulator::GetOFMessage(const lc::Time current_time) { return of_buffer_.GetMessage(current_time); } +boost::optional LiveMeasurementSimulator::GetDepthOdometryMessage(const lc::Time current_time) { + return depth_odometry_buffer_.GetMessage(current_time); +} boost::optional LiveMeasurementSimulator::GetVLMessage(const lc::Time current_time) { return vl_buffer_.GetMessage(current_time); } diff --git a/tools/graph_bag/src/parameter_reader.cc b/tools/graph_bag/src/parameter_reader.cc index d487e34773..8be97801dd 100644 --- a/tools/graph_bag/src/parameter_reader.cc +++ b/tools/graph_bag/src/parameter_reader.cc @@ -35,6 +35,7 @@ void LoadLiveMeasurementSimulatorParams(config_reader::ConfigReader& config, con LiveMeasurementSimulatorParams& params) { LoadMessageBufferParams("imu", config, params.imu); LoadMessageBufferParams("flight_mode", config, params.flight_mode); + LoadMessageBufferParams("depth_odometry", config, params.depth_odometry); LoadMessageBufferParams("of", config, params.of); LoadMessageBufferParams("vl", config, params.vl); LoadMessageBufferParams("ar", config, params.ar); diff --git a/tools/graph_bag/src/utilities.cc b/tools/graph_bag/src/utilities.cc index 0c6fe27ad9..05863da5e3 100644 --- a/tools/graph_bag/src/utilities.cc +++ b/tools/graph_bag/src/utilities.cc @@ -96,10 +96,10 @@ boost::optional CreateFeatureTrackImage(const sensor_msgs return feature_track_image->toImageMsg(); } -cv::Point Distort(const Eigen::Vector2d& undistorted_point, const camera::CameraParameters& params) { +cv::Point2f Distort(const Eigen::Vector2d& undistorted_point, const camera::CameraParameters& params) { Eigen::Vector2d distorted_point; params.Convert(undistorted_point, &distorted_point); - return cv::Point(distorted_point.x(), distorted_point.y()); + return cv::Point2f(distorted_point.x(), distorted_point.y()); } std::vector SmartFactors(const graph_localizer::GraphLocalizer& graph) { diff --git a/tools/graph_bag/tools/convert_depth_msg.cc b/tools/graph_bag/tools/convert_depth_msg.cc new file mode 100644 index 0000000000..b6c918492c --- /dev/null +++ b/tools/graph_bag/tools/convert_depth_msg.cc @@ -0,0 +1,104 @@ +/* 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. + */ + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace po = boost::program_options; + +// TODO(rsoussan): Is this necessary? +struct null_deleter { + void operator()(void const*) const {} +}; + +void ConvertBag(const std::string& input_bagfile, const std::string& output_bagfile) { + const std::string depth_topic = static_cast(TOPIC_HARDWARE_PICOFLEXX_PREFIX) + + static_cast(TOPIC_HARDWARE_NAME_HAZ_CAM) + + static_cast(TOPIC_HARDWARE_PICOFLEXX_SUFFIX_EXTENDED); + const std::string depth_intensity_topic = + static_cast(TOPIC_HARDWARE_PICOFLEXX_PREFIX) + static_cast(TOPIC_HARDWARE_NAME_HAZ_CAM) + + static_cast(TOPIC_HARDWARE_PICOFLEXX_SUFFIX_EXTENDED) + "/amplitude_int"; + + rosbag::Bag input_bag(input_bagfile, rosbag::bagmode::Read); + rosbag::Bag output_bag(output_bagfile, rosbag::bagmode::Write); + rosbag::View view(input_bag); + for (const rosbag::MessageInstance msg : view) { + if (graph_bag::string_ends_with(msg.getTopic(), depth_topic)) { + const ff_msgs::PicoflexxIntermediateData::ConstPtr& depth_msg = + msg.instantiate(); + // TODO(rsoussan): Unify this with pico_driver code + cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare( + sensor_msgs::ImageConstPtr(&depth_msg->raw, null_deleter()), sensor_msgs::image_encodings::TYPE_32FC4); + std::vector layers(4); + cv::split(cv_ptr->image, layers); + constexpr double amplitude_factor = 100; + cv::Mat intenisty_image; + layers[1].convertTo(intenisty_image, CV_16UC1, amplitude_factor, 0); + std_msgs::Header header; + header.stamp = depth_msg->header.stamp; + header.frame_id = depth_msg->header.frame_id; + cv_bridge::CvImage intensity_image_msg(header, sensor_msgs::image_encodings::MONO16, intenisty_image); + output_bag.write(depth_intensity_topic, msg.getTime(), intensity_image_msg.toImageMsg()); + } else { + output_bag.write(msg.getTopic(), msg.getTime(), msg); + } + } +} + +int main(int argc, char** argv) { + po::options_description desc("Converts depth topic in bagfile to intensity image."); + desc.add_options()("help", "produce help message")("bagfile", po::value()->required(), "Bagfile"); + po::positional_options_description p; + p.add("bagfile", 1); + po::variables_map vm; + try { + po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm); + po::notify(vm); + } catch (std::exception& e) { + std::cerr << "Error: " << e.what() << "\n"; + return 1; + } + + if (vm.count("help")) { + std::cout << desc << "\n"; + return 1; + } + + const std::string input_bag = vm["bagfile"].as(); + if (!boost::filesystem::exists(input_bag)) { + LogFatal("Bagfile " << input_bag << " not found."); + } + boost::filesystem::path input_bag_path(input_bag); + boost::filesystem::path output_bag_path = + input_bag_path.parent_path() / boost::filesystem::path(input_bag_path.stem().string() + "_depth_converted.bag"); + + ConvertBag(input_bag, output_bag_path.string()); +} diff --git a/tools/graph_bag/tools/run_depth_odometry_adder.cc b/tools/graph_bag/tools/run_depth_odometry_adder.cc new file mode 100644 index 0000000000..17656e896e --- /dev/null +++ b/tools/graph_bag/tools/run_depth_odometry_adder.cc @@ -0,0 +1,94 @@ +/* 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. + */ + +#include +#include +#include +#include + +#include +#include + +namespace po = boost::program_options; +namespace lc = localization_common; + +int main(int argc, char** argv) { + std::string output_bagfile; + std::string robot_config_file; + std::string world; + bool save_all_topics = false; + std::string config_path_prefix; + po::options_description desc("Adds depth odometry relative poses to a new bag file."); + desc.add_options()("help,h", "produce help message")( + "bagfile", po::value()->required(), + "Input bagfile containing point cloud and image messages for a depth camera.")( + "config-path,c", po::value()->required(), "Path to config directory.")( + "robot-config-file,r", po::value(&robot_config_file)->default_value("config/robots/bumble.config"), + "Robot config file")("world,w", po::value(&world)->default_value("iss"), "World name")( + "output-bagfile,o", po::value(&output_bagfile)->default_value(""), + "Output bagfile, defaults to input_bag + with_depth_odometry.bag")( + "save-all-topics,s", po::bool_switch(&save_all_topics), + "Save all topics in input bagfile to bagfile with depth odometry. Otherwise just save localization relevant (but " + "no raw data) topics.")("config-path-prefix,p", + po::value(&config_path_prefix)->default_value("localization/"), + "Config path prefix"); + po::positional_options_description p; + p.add("bagfile", 1); + p.add("config-path", 1); + po::variables_map vm; + try { + po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm); + if (vm.count("help") || (argc <= 1)) { + std::cout << desc << "\n"; + return 1; + } + po::notify(vm); + } catch (std::exception& e) { + std::cerr << "Error: " << e.what() << "\n"; + return 1; + } + + const std::string input_bag = vm["bagfile"].as(); + const std::string config_path = vm["config-path"].as(); + + // Only pass program name to free flyer so that boost command line options + // are ignored when parsing gflags. + int ff_argc = 1; + ff_common::InitFreeFlyerApplication(&ff_argc, &argv); + + if (!boost::filesystem::exists(input_bag)) { + LogFatal("Bagfile " << input_bag << " not found."); + } + + boost::filesystem::path input_bag_path(input_bag); + if (vm["output-bagfile"].defaulted()) { + boost::filesystem::path output_bag_path = + boost::filesystem::current_path() / + boost::filesystem::path(input_bag_path.stem().string() + "_with_depth_odometry.bag"); + output_bagfile = output_bag_path.string(); + } + lc::SetEnvironmentConfigs(config_path, world, robot_config_file); + config_reader::ConfigReader config; + config.AddFile("geometry.config"); + if (!config.ReadFiles()) { + LogFatal("Failed to read config files."); + } + + graph_bag::DepthOdometryAdder depth_odometry_adder(input_bag, output_bagfile, save_all_topics, config_path_prefix); + depth_odometry_adder.AddDepthOdometry(); +} diff --git a/tools/graph_bag/tools/run_graph_bag.cc b/tools/graph_bag/tools/run_graph_bag.cc index 0543a3900b..e63d641f15 100644 --- a/tools/graph_bag/tools/run_graph_bag.cc +++ b/tools/graph_bag/tools/run_graph_bag.cc @@ -40,7 +40,8 @@ int main(int argc, char** argv) { bool use_image_features; std::string graph_config_path_prefix; po::options_description desc("Runs graph localization on a bagfile and saves the results to a new bagfile."); - desc.add_options()("help", "produce help message")("bagfile", po::value()->required(), "Input bagfile")( + desc.add_options()("help,h", "produce help message")("bagfile", po::value()->required(), + "Input bagfile")( "map-file", po::value()->required(), "Map file")("config-path,c", po::value()->required(), "Config path")( "image-topic,i", po::value(&image_topic)->default_value("mgt/img_sampler/nav_cam/image_record"), @@ -60,17 +61,16 @@ int main(int argc, char** argv) { po::variables_map vm; try { po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm); + if (vm.count("help") || (argc <= 1)) { + std::cout << desc << "\n"; + return 1; + } po::notify(vm); } catch (std::exception& e) { std::cerr << "Error: " << e.what() << "\n"; return 1; } - if (vm.count("help")) { - std::cout << desc << "\n"; - return 1; - } - const std::string input_bag = vm["bagfile"].as(); const std::string map_file = vm["map-file"].as(); const std::string config_path = vm["config-path"].as(); diff --git a/tools/interactive_marker_teleop/CMakeLists.txt b/tools/interactive_marker_teleop/CMakeLists.txt new file mode 100644 index 0000000000..c374a523da --- /dev/null +++ b/tools/interactive_marker_teleop/CMakeLists.txt @@ -0,0 +1,83 @@ +# 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. + +cmake_minimum_required(VERSION 3.0.2) +project(interactive_marker_teleop) + +if (BUILD_LOC_RVIZ_PLUGINS) + + ## Compile as C++14, supported in ROS Kinetic and newer + add_compile_options(-std=c++14) + + ## Find catkin macros and libraries + find_package(catkin2 REQUIRED COMPONENTS + ff_hw_msgs + ff_msgs + ff_util + roscpp + interactive_markers + visualization_msgs + tf2 + tf2_ros + tf2_geometry_msgs + ) + + ################################### + ## catkin specific configuration ## + ################################### + ## The catkin_package macro generates cmake config files for your package + ## Declare things to be passed to dependent projects + ## INCLUDE_DIRS: uncomment this if your package contains header files + ## LIBRARIES: libraries you create in this project that dependent projects also need + ## CATKIN_DEPENDS: catkin_packages dependent projects also need + ## DEPENDS: system dependencies of this project that dependent projects also need + catkin_package( + INCLUDE_DIRS include + ) + + ########### + ## Build ## + ########### + + ## Specify additional locations of header files + ## Your package locations should be listed before other locations + include_directories( + include + ${catkin_INCLUDE_DIRS} + ) + + ## Declare a C++ executable + add_executable(${PROJECT_NAME} src/interactive_marker_teleop.cpp) + add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} + ) + + ############# + ## Install ## + ############# + + ## Mark executables for installation + ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html + install(TARGETS ${PROJECT_NAME} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + +else (BUILD_LOC_RVIZ_PLUGINS) + find_package(catkin REQUIRED COMPONENTS) + catkin_package() +endif (BUILD_LOC_RVIZ_PLUGINS) diff --git a/tools/interactive_marker_teleop/include/interactive_marker_teleop.h b/tools/interactive_marker_teleop/include/interactive_marker_teleop.h new file mode 100644 index 0000000000..a39d63e01a --- /dev/null +++ b/tools/interactive_marker_teleop/include/interactive_marker_teleop.h @@ -0,0 +1,65 @@ +/* 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. + */ + +#ifndef INTERACTIVE_MARKER_TELEOP_H_ +#define INTERACTIVE_MARKER_TELEOP_H_ + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +class InteractiveMarkerTeleop { + public: + explicit InteractiveMarkerTeleop(ros::NodeHandle& nh); + + visualization_msgs::Marker makeMarker(const std::string marker_type); + + visualization_msgs::InteractiveMarkerControl& makeBoxControl(visualization_msgs::InteractiveMarker& msg); + + void sendMobilityCommand(std::string command, const geometry_msgs::Pose& desired_pose); + + void sendMobilityCommand(std::string command); + + void snapMarkerToAstrobee(); + + void processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); + + void make6DofMarker(unsigned int interaction_mode); + + private: + ros::NodeHandle nh; + + std::shared_ptr server_; + interactive_markers::MenuHandler menu_handler_; + + ros::Publisher cmd_publisher_; + ros::Subscriber ack_subscriber_; + + tf2_ros::Buffer tfBuffer_; + std::shared_ptr tfListener_; +}; + +#endif // INTERACTIVE_MARKER_TELEOP_H_ diff --git a/tools/interactive_marker_teleop/package.xml b/tools/interactive_marker_teleop/package.xml new file mode 100644 index 0000000000..55b8633aa4 --- /dev/null +++ b/tools/interactive_marker_teleop/package.xml @@ -0,0 +1,35 @@ + + + interactive_marker_teleop + 1.0.0 + The interactive_marker_teleop package. Used to send movement commands from rviz. + + Apache License, Version 2.0 + + + Astrobee Flight Software + + + Astrobee Flight Software + + catkin + ff_hw_msgs + ff_msgs + ff_util + roscpp + interactive_markers + visualization_msgs + tf2 + tf2_ros + tf2_geometry_msgs + ff_hw_msgs + ff_msgs + ff_util + roscpp + interactive_markers + visualization_msgs + tf2 + tf2_ros + tf2_geometry_msgs + + diff --git a/tools/interactive_marker_teleop/readme.md b/tools/interactive_marker_teleop/readme.md new file mode 100644 index 0000000000..a8d2bfede5 --- /dev/null +++ b/tools/interactive_marker_teleop/readme.md @@ -0,0 +1,11 @@ +\page interactive_marker_teleop Interactive Marker Teleop + +The interactive marker tool overlays a control in rviz to command robot movements and display feedback. + +1. Run the tool +```rosrun interactive_marker_teleop interactive_marker_teleop``` +2. Add to rviz using the `Add` button in the displays panel and choose `By Topic`, then `InteractiveMarkers` + +Move the marker to a desired location and right click the marker to initiate commands from the dropdown menu. Feedback can be found in the console if the command is not valid. + +Clicking the marker can be a challenge if there are objects in the way. Disabling "Debug/zones", for example, will help. diff --git a/tools/interactive_marker_teleop/src/interactive_marker_teleop.cpp b/tools/interactive_marker_teleop/src/interactive_marker_teleop.cpp new file mode 100644 index 0000000000..4661d8a94b --- /dev/null +++ b/tools/interactive_marker_teleop/src/interactive_marker_teleop.cpp @@ -0,0 +1,289 @@ +/* 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. + */ + +#include + +#include + +namespace vm = visualization_msgs; + +InteractiveMarkerTeleop::InteractiveMarkerTeleop(ros::NodeHandle& nh) : nh(nh) { + // Start tf listener + tfListener_ = std::shared_ptr(new tf2_ros::TransformListener(tfBuffer_)); + + // Make the marker + server_.reset(new interactive_markers::InteractiveMarkerServer("interactive_marker_teleop", "", false)); + ros::Duration(0.1).sleep(); + menu_handler_.insert("Snap to Astrobee", + std::bind(&InteractiveMarkerTeleop::processFeedback, this, std::placeholders::_1)); + menu_handler_.insert("Go to Position", + std::bind(&InteractiveMarkerTeleop::processFeedback, this, std::placeholders::_1)); + menu_handler_.insert("Dock", std::bind(&InteractiveMarkerTeleop::processFeedback, this, std::placeholders::_1)); + menu_handler_.insert("Undock", std::bind(&InteractiveMarkerTeleop::processFeedback, this, std::placeholders::_1)); + menu_handler_.insert("Stop", std::bind(&InteractiveMarkerTeleop::processFeedback, this, std::placeholders::_1)); + + // menu_handler_.insert("Add to plan", &InteractiveMarkerTeleop::processFeedback, this, std::placeholders::_1); // + // TODO(jdekarske) would be cool to add a new marker for each station menu_handler_.insert("Remove from plan", + // &InteractiveMarkerTeleop::processFeedback, this, std::placeholders::_1); + + make6DofMarker(vm::InteractiveMarkerControl::MOVE_ROTATE_3D); + server_->applyChanges(); + + snapMarkerToAstrobee(); + // publish and acknowledge command actions + cmd_publisher_ = nh.advertise(TOPIC_COMMAND, 10, true); + // ack_subscriber_ = nh.subscribe(TOPIC_MANAGEMENT_ACK, 10, &AckCallback); // TODO(jdekarske) status goes in the + // marker text +} + +vm::Marker InteractiveMarkerTeleop::makeMarker(const std::string marker_type) { + vm::Marker marker; + + // make it yellowish and see through + marker.color.r = 1.0; + marker.color.g = 1.0; + marker.color.b = 0.6; + marker.color.a = 0.8; + marker.scale.x = 1.0; + marker.scale.y = 1.0; + marker.scale.z = 1.0; + + // return a different marker using meshes from astrobee_media + if (marker_type == "cube") { + marker.type = vm::Marker::CUBE; + marker.scale.x = 0.3175; + marker.scale.y = 0.3175; + marker.scale.z = 0.3175; + } else if (marker_type == "body") { + marker.type = vm::Marker::MESH_RESOURCE; + marker.mesh_resource = "package://astrobee_freeflyer/meshes/body.dae"; + } else if (marker_type == "pmc_left") { + marker.type = vm::Marker::MESH_RESOURCE; + marker.mesh_resource = "package://astrobee_freeflyer/meshes/pmc_skin_.dae"; + } else if (marker_type == "pmc_right") { + marker.type = vm::Marker::MESH_RESOURCE; + marker.mesh_resource = "package://astrobee_freeflyer/meshes/pmc_skin_.dae"; + marker.pose.orientation.z = 1.0; + } + + return marker; +} + +vm::InteractiveMarkerControl& InteractiveMarkerTeleop::makeBoxControl(vm::InteractiveMarker& msg) { + vm::InteractiveMarkerControl control; + control.always_visible = true; + control.markers.push_back(makeMarker("body")); + control.markers.push_back(makeMarker("pmc_left")); + control.markers.push_back(makeMarker("pmc_right")); + msg.controls.push_back(control); + + return msg.controls.back(); +} + +void InteractiveMarkerTeleop::sendMobilityCommand(std::string command, const geometry_msgs::Pose& desired_pose) { + if (command == ff_msgs::CommandConstants::CMD_NAME_SIMPLE_MOVE6DOF) { + // Make ros command message to send to the executive. see `simple_move.cc` + ff_msgs::CommandStamped move_cmd; + move_cmd.header.stamp = ros::Time::now(); + move_cmd.cmd_name = ff_msgs::CommandConstants::CMD_NAME_SIMPLE_MOVE6DOF; + move_cmd.cmd_id = "interactive_marker" + std::to_string(move_cmd.header.stamp.sec); + move_cmd.cmd_src = "interactive_marker"; + move_cmd.subsys_name = "Astrobee"; + + // Move command has 4 arguements; frame, xyz, xyz tolerance, and rotation + move_cmd.args.resize(4); + move_cmd.args[0].data_type = ff_msgs::CommandArg::DATA_TYPE_STRING; + move_cmd.args[0].s = "world"; + + // Set location where you want Astrobee to go to + move_cmd.args[1].data_type = ff_msgs::CommandArg::DATA_TYPE_VEC3d; + move_cmd.args[1].vec3d[0] = desired_pose.position.x; // x + move_cmd.args[1].vec3d[1] = desired_pose.position.y; // y + move_cmd.args[1].vec3d[2] = desired_pose.position.z; // z + + // "Tolerance not used!" + move_cmd.args[2].data_type = ff_msgs::CommandArg::DATA_TYPE_VEC3d; + move_cmd.args[2].vec3d[0] = 0; + move_cmd.args[2].vec3d[1] = 0; + move_cmd.args[2].vec3d[2] = 0; + + // Target attitude, quaternion, only the first 4 values are used + move_cmd.args[3].data_type = ff_msgs::CommandArg::DATA_TYPE_MAT33f; + move_cmd.args[3].mat33f[0] = desired_pose.orientation.x; + move_cmd.args[3].mat33f[1] = desired_pose.orientation.y; + move_cmd.args[3].mat33f[2] = desired_pose.orientation.z; + move_cmd.args[3].mat33f[3] = desired_pose.orientation.w; + move_cmd.args[3].mat33f[4] = 0; + move_cmd.args[3].mat33f[5] = 0; + move_cmd.args[3].mat33f[6] = 0; + move_cmd.args[3].mat33f[7] = 0; + move_cmd.args[3].mat33f[8] = 0; + + // Send command + cmd_publisher_.publish(move_cmd); + } else { + ROS_ERROR("only simplemove supported with pose"); + } +} + +void InteractiveMarkerTeleop::sendMobilityCommand(std::string command) { + ff_msgs::CommandStamped cmd; + cmd.header.stamp = ros::Time::now(); + cmd.subsys_name = "Astrobee"; + cmd.cmd_name = command; + cmd.cmd_id = command; + + if (command == ff_msgs::CommandConstants::CMD_NAME_DOCK) { + // Dock has one argument + cmd.args.resize(1); + cmd.args[0].data_type = ff_msgs::CommandArg::DATA_TYPE_INT; + cmd.args[0].i = 1; // TODO(jdekarske) support other berth + } + + cmd_publisher_.publish(cmd); +} + +void InteractiveMarkerTeleop::snapMarkerToAstrobee() { + // get astrobee position + geometry_msgs::TransformStamped t; + t = tfBuffer_.lookupTransform("world", "body", ros::Time(0)); + // set marker pose + geometry_msgs::Pose p; + p.position.x = t.transform.translation.x; + p.position.y = t.transform.translation.y; + p.position.z = t.transform.translation.z; + p.orientation = t.transform.rotation; + server_->setPose("astrobee_teleop", p); + server_->applyChanges(); +} + +void InteractiveMarkerTeleop::processFeedback(const vm::InteractiveMarkerFeedbackConstPtr& feedback) { + // TODO(jdekarske) This is a switch for adding a keepout zone check (see below) + switch (feedback->event_type) { + case vm::InteractiveMarkerFeedback::MENU_SELECT: + switch (feedback->menu_entry_id) { + case 1: // snap to astrobee + { + snapMarkerToAstrobee(); + break; + } + case 2: // move to desired position + { + sendMobilityCommand(ff_msgs::CommandConstants::CMD_NAME_SIMPLE_MOVE6DOF, feedback->pose); + break; + } + case 3: // dock + { + sendMobilityCommand(ff_msgs::CommandConstants::CMD_NAME_DOCK); + break; + } + case 4: // undock + { + sendMobilityCommand(ff_msgs::CommandConstants::CMD_NAME_UNDOCK); + break; + } + case 5: // stop + { + sendMobilityCommand(ff_msgs::CommandConstants::CMD_NAME_STOP_ALL_MOTION); + break; + } + default: + break; + } + break; + + // TODO(jdekarske) check for keep out zones here and turn astrobee marker red if invalid + // case vm::InteractiveMarkerFeedback::POSE_UPDATE: + // ROS_INFO_STREAM(s.str() << ": pose changed" + // << "\nposition = " + // << feedback->pose.position.x + // << ", " << feedback->pose.position.y + // << ", " << feedback->pose.position.z + // << "\norientation = " + // << feedback->pose.orientation.w + // << ", " << feedback->pose.orientation.x + // << ", " << feedback->pose.orientation.y + // << ", " << feedback->pose.orientation.z + // << "\nframe: " << feedback->header.frame_id + // << " time: " << feedback->header.stamp.sec << "sec, " + // << feedback->header.stamp.nsec << " nsec"); + // break; + // TODO(jdekarske) check for keep out zones here and turn astrobee marker red if invalid + } +} + +void InteractiveMarkerTeleop::make6DofMarker(unsigned int interaction_mode) { + vm::InteractiveMarker int_marker; + int_marker.header.frame_id = "world"; + int_marker.scale = 1; + + int_marker.name = "astrobee_teleop"; + int_marker.description = "Astrobee Command"; + + // insert a box + makeBoxControl(int_marker); + int_marker.controls[0].interaction_mode = interaction_mode; + + vm::InteractiveMarkerControl control; + + tf2::Quaternion control_orientation(1.0, 0.0, 0.0, 1.0); + control_orientation.normalize(); + tf2::convert(control_orientation, control.orientation); + control.name = "rotate_x"; + control.interaction_mode = vm::InteractiveMarkerControl::ROTATE_AXIS; + int_marker.controls.push_back(control); + control.name = "move_x"; + control.interaction_mode = vm::InteractiveMarkerControl::MOVE_AXIS; + int_marker.controls.push_back(control); + + control_orientation = tf2::Quaternion(0.0, 1.0, 0.0, 1.0); + control_orientation.normalize(); + tf2::convert(control_orientation, control.orientation); + control.name = "rotate_z"; + control.interaction_mode = vm::InteractiveMarkerControl::ROTATE_AXIS; + int_marker.controls.push_back(control); + control.name = "move_z"; + control.interaction_mode = vm::InteractiveMarkerControl::MOVE_AXIS; + int_marker.controls.push_back(control); + + control_orientation = tf2::Quaternion(0.0, 0.0, 1.0, 1.0); + control_orientation.normalize(); + tf2::convert(control_orientation, control.orientation); + control.name = "rotate_y"; + control.interaction_mode = vm::InteractiveMarkerControl::ROTATE_AXIS; + int_marker.controls.push_back(control); + control.name = "move_y"; + control.interaction_mode = vm::InteractiveMarkerControl::MOVE_AXIS; + int_marker.controls.push_back(control); + + server_->insert(int_marker); + server_->setCallback(int_marker.name, + std::bind(&InteractiveMarkerTeleop::processFeedback, this, std::placeholders::_1)); + menu_handler_.apply(*server_, int_marker.name); +} + +int main(int argc, char** argv) { + ros::init(argc, argv, "interactive_marker_teleop"); + ros::NodeHandle nh; + + InteractiveMarkerTeleop teleop(nh); + + ros::spin(); + + return 0; +} diff --git a/tools/localization_rviz_plugins/CMakeLists.txt b/tools/localization_rviz_plugins/CMakeLists.txt index ab41a3037d..28424cf8fe 100644 --- a/tools/localization_rviz_plugins/CMakeLists.txt +++ b/tools/localization_rviz_plugins/CMakeLists.txt @@ -34,6 +34,8 @@ if (USE_ROS AND BUILD_LOC_RVIZ_PLUGINS) rviz ) + find_package(pcl_conversions REQUIRED) + # For Qt set(CMAKE_AUTOMOC ON) find_package(Qt5 ${rviz_QT_VERSION} REQUIRED Core Widgets) @@ -41,7 +43,7 @@ if (USE_ROS AND BUILD_LOC_RVIZ_PLUGINS) catkin_package( LIBRARIES ${PROJECT_NAME} ${GLOG_LIBRARIES} ${GTSAM_LIBRARIES} - INCLUDE_DIRS ${GTSAM_INCLUDE_DIR} ${GLOG_INCLUDE_DIRS} + INCLUDE_DIRS ${GTSAM_INCLUDE_DIR} ${GLOG_INCLUDE_DIRS} ${pcl_conversions_INCLUDE_DIRS} CATKIN_DEPENDS roscpp localization_common ff_util graph_localizer graph_bag rviz ) @@ -52,17 +54,20 @@ if (USE_ROS AND BUILD_LOC_RVIZ_PLUGINS) include_directories( include ${catkin_INCLUDE_DIRS} + ${pcl_conversions_INCLUDE_DIRS} ${Qt5Core_INCLUDE_DIRS} ${Qt5Gui_INCLUDE_DIRS} ) # Declare C++ libraries add_library(${PROJECT_NAME} + src/depth_odometry_display.cc src/localization_graph_display.cc src/localization_graph_panel.cc src/pose_display.cc src/utilities.cc src/imu_augmentor_display.cc + src/slider_property.cc src/sparse_mapping_display.cc ) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) diff --git a/tools/localization_rviz_plugins/nodelet_plugins.xml b/tools/localization_rviz_plugins/nodelet_plugins.xml index e10511248d..c4b3f276d7 100644 --- a/tools/localization_rviz_plugins/nodelet_plugins.xml +++ b/tools/localization_rviz_plugins/nodelet_plugins.xml @@ -1,4 +1,11 @@ + + + Depth Odometry Display + + diff --git a/tools/localization_rviz_plugins/src/depth_odometry_display.cc b/tools/localization_rviz_plugins/src/depth_odometry_display.cc new file mode 100644 index 0000000000..835b7072d9 --- /dev/null +++ b/tools/localization_rviz_plugins/src/depth_odometry_display.cc @@ -0,0 +1,277 @@ +/* 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. + */ +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "depth_odometry_display.h" // NOLINT +#include "utilities.h" // NOLINT + +namespace localization_rviz_plugins { +namespace gb = graph_bag; +namespace lc = localization_common; +namespace lm = localization_measurements; +namespace mc = msg_conversions; + +cv::Point2f CvPoint(const Eigen::Vector2d& point) { return cv::Point2f(point.x(), point.y()); } + +DepthOdometryDisplay::DepthOdometryDisplay() { + int ff_argc = 1; + char argv[] = "depth_odometry_display"; + char* argv_ptr = &argv[0]; + char** argv_ptr_ptr = &argv_ptr; + ff_common::InitFreeFlyerApplication(&ff_argc, &argv_ptr_ptr); + config_reader::ConfigReader config; + config.AddFile("cameras.config"); + config.AddFile("geometry.config"); + config.AddFile("transforms.config"); + if (!config.ReadFiles()) { + LogFatal("Failed to read config files."); + } + camera_params_.reset(new camera::CameraParameters(&config, "haz_cam")); + const auto focal_lengths = camera_params_->GetFocalVector(); + const auto distortion_params = camera_params_->GetDistortion(); + const auto principal_points = camera_params_->GetOpticalOffset(); + intrinsics_ = cv::Mat::zeros(3, 3, cv::DataType::type); + intrinsics_.at(0, 0) = focal_lengths[0]; + intrinsics_.at(1, 1) = focal_lengths[1]; + intrinsics_.at(0, 2) = principal_points[0]; + intrinsics_.at(1, 2) = principal_points[1]; + intrinsics_.at(2, 2) = 1; + distortion_params_ = cv::Mat(4, 1, cv::DataType::type); + for (int i = 0; i < distortion_params.size(); ++i) { + distortion_params_.at(i, 0) = distortion_params[i]; + } + + correspondence_index_slider_.reset(new rviz::SliderProperty("Select Correspondence", 0, "Select Correspondence.", + this, SLOT(createSingleCorrespondenceDisplays()))); + + image_transport::ImageTransport image_transport(nh_); + const std::string image_topic = static_cast(TOPIC_HARDWARE_PICOFLEXX_PREFIX) + + static_cast(TOPIC_HARDWARE_NAME_HAZ_CAM) + + static_cast(TOPIC_HARDWARE_PICOFLEXX_SUFFIX_EXTENDED) + + static_cast(TOPIC_HARDWARE_PICOFLEXX_SUFFIX_AMPLITUDE_IMAGE); + image_sub_ = image_transport.subscribe(image_topic, 10, &DepthOdometryDisplay::imageCallback, this); + const std::string point_cloud_topic = static_cast(TOPIC_HARDWARE_PICOFLEXX_PREFIX) + + static_cast(TOPIC_HARDWARE_NAME_HAZ_CAM) + + static_cast(TOPIC_HARDWARE_PICOFLEXX_SUFFIX); + point_cloud_sub_ = nh_.subscribe( + point_cloud_topic, 10, &DepthOdometryDisplay::pointCloudCallback, this, ros::TransportHints().tcpNoDelay()); + single_correspondence_image_pub_ = image_transport.advertise("/depth_odom/single_correspondence_image", 1); + projection_and_optical_flow_image_pub_ = + image_transport.advertise("/depth_odom/projection_and_optical_flow_image", 1); + source_correspondence_point_pub_ = nh_.advertise("source_point_with_correspondence", 10); + target_correspondence_point_pub_ = nh_.advertise("target_point_with_correspondence", 10); + source_point_cloud_pub_ = nh_.advertise("source_cloud", 10); + target_point_cloud_pub_ = nh_.advertise("target_cloud", 10); +} + +void DepthOdometryDisplay::onInitialize() { MFDClass::onInitialize(); } + +void DepthOdometryDisplay::reset() { + MFDClass::reset(); + clearDisplay(); +} + +void DepthOdometryDisplay::clearDisplay() {} + +void DepthOdometryDisplay::imageCallback(const sensor_msgs::ImageConstPtr& image_msg) { + img_buffer_.Add(lc::TimeFromHeader(image_msg->header), image_msg); +} + +void DepthOdometryDisplay::pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& point_cloud_msg) { + pcl::PointCloud::Ptr point_cloud(new pcl::PointCloud()); + pcl::fromROSMsg(*point_cloud_msg, *point_cloud); + point_cloud_buffer_.Add(lc::TimeFromHeader(point_cloud_msg->header), point_cloud); +} + +void DepthOdometryDisplay::processMessage(const ff_msgs::DepthOdometry::ConstPtr& depth_odometry_msg) { + latest_depth_odometry_measurement_ = lm::MakeDepthOdometryMeasurement(*depth_odometry_msg); + createSingleCorrespondenceDisplays(); + createProjectionAndOpticalFlowImage(*latest_depth_odometry_measurement_); +} + +// TODO(rsoussan): Move this to vision_common? +cv::Point2f DepthOdometryDisplay::projectPoint(const Eigen::Vector3d& point_3d) { + cv::Mat zero_r(cv::Mat::eye(3, 3, cv::DataType::type)); + cv::Mat zero_t(cv::Mat::zeros(3, 1, cv::DataType::type)); + std::vector projected_points; + std::vector object_points; + object_points.emplace_back(cv::Point3d(point_3d.x(), point_3d.y(), point_3d.z())); + cv::projectPoints(object_points, zero_r, zero_t, intrinsics_, distortion_params_, projected_points); + return projected_points[0]; +} + +void DepthOdometryDisplay::createProjectionAndOpticalFlowImage(const lm::DepthOdometryMeasurement& depth_odometry) { + if (!depth_odometry.correspondences.valid_image_points) return; + + const lc::Time source_time = depth_odometry.odometry.source_time; + const lc::Time target_time = depth_odometry.odometry.target_time; + const auto source_image_msg = img_buffer_.Get(source_time); + const auto target_image_msg = img_buffer_.Get(target_time); + if (!source_image_msg || !target_image_msg) return; + + auto source_image_measurement = lm::MakeImageMeasurement(*source_image_msg, sensor_msgs::image_encodings::RGB8); + if (!source_image_measurement) return; + auto& source_image = source_image_measurement->image; + + auto target_image_measurement = lm::MakeImageMeasurement(*target_image_msg, sensor_msgs::image_encodings::RGB8); + if (!target_image_measurement) return; + auto& target_image = target_image_measurement->image; + + // Draw optical flow tracks in target image, add projected 3D points as well + cv_bridge::CvImage projection_and_optical_flow_image; + projection_and_optical_flow_image.encoding = sensor_msgs::image_encodings::RGB8; + const int rows = target_image.rows; + const int cols = target_image.cols; + projection_and_optical_flow_image.image = cv::Mat(rows * 2, cols, CV_8UC3, cv::Scalar(0, 0, 0)); + + const auto& correspondences = depth_odometry.correspondences; + const cv::Point2f rectangle_offset(3, 3); + for (int i = 0; i < correspondences.source_image_points.size(); ++i) { + const cv::Point2f source_image_point = CvPoint(correspondences.source_image_points[i]); + const Eigen::Vector3d& target_3d_point = correspondences.target_3d_points[i]; + const Eigen::Vector3d frame_changed_target_3d_point = + depth_odometry.odometry.sensor_F_source_T_target.pose * target_3d_point; + const auto projected_target_point = projectPoint(frame_changed_target_3d_point); + const cv::Point2f target_image_point = CvPoint(correspondences.target_image_points[i]); + // Draw source point and projected target point in source image + cv::circle(source_image, source_image_point, 1 /* Radius*/, cv::Scalar(0, 255, 0), -1 /*Filled*/, 8); + cv::rectangle(source_image, source_image_point - rectangle_offset, source_image_point + rectangle_offset, + cv::Scalar(0, 255, 0), 1); + cv::circle(source_image, projected_target_point, 1 /* Radius*/, cv::Scalar(255, 0, 0), -1 /*Filled*/, 8); + cv::line(source_image, source_image_point, projected_target_point, cv::Scalar(255, 0, 0), 2, 8, 0); + // Draw target point and optical flow line in target image + cv::circle(target_image, target_image_point, 1 /* Radius*/, cv::Scalar(0, 255, 0), -1 /*Filled*/, 8); + cv::rectangle(target_image, target_image_point - rectangle_offset, target_image_point + rectangle_offset, + cv::Scalar(0, 255, 0), 1); + cv::circle(target_image, source_image_point, 1 /* Radius*/, cv::Scalar(255, 0, 0), -1 /*Filled*/, 8); + cv::line(target_image, target_image_point, source_image_point, cv::Scalar(255, 0, 0), 2, 8, 0); + } + source_image.copyTo(projection_and_optical_flow_image.image(cv::Rect(0, 0, cols, rows))); + target_image.copyTo(projection_and_optical_flow_image.image(cv::Rect(0, rows, cols, rows))); + projection_and_optical_flow_image_pub_.publish(projection_and_optical_flow_image.toImageMsg()); + // Account for time offset when erasing point cloud buffer + point_cloud_buffer_.EraseUpTo(source_time - 0.05); + img_buffer_.EraseUpTo(source_time); +} + +void DepthOdometryDisplay::createSingleCorrespondenceDisplays() { + if (!latest_depth_odometry_measurement_) return; + const auto& depth_odometry = *latest_depth_odometry_measurement_; + clearDisplay(); + // Publish 3d correspondences + correspondence_index_slider_->setMaximum(depth_odometry.correspondences.target_3d_points.size() - 1); + const int correspondence_index = correspondence_index_slider_->getInt(); + const lc::Time source_time = depth_odometry.odometry.source_time; + const lc::Time target_time = depth_odometry.odometry.target_time; + const Eigen::Vector3d& source_3d_point = depth_odometry.correspondences.source_3d_points[correspondence_index]; + const Eigen::Vector3d& target_3d_point = depth_odometry.correspondences.target_3d_points[correspondence_index]; + publishCorrespondencePoints(source_3d_point, target_3d_point, source_time, target_time); + + // Publish image space correspondences if available + if (!depth_odometry.correspondences.valid_image_points) return; + const auto source_image_msg = img_buffer_.Get(source_time); + const auto target_image_msg = img_buffer_.Get(target_time); + if (!source_image_msg || !target_image_msg) return; + + auto source_image_measurement = lm::MakeImageMeasurement(*source_image_msg, sensor_msgs::image_encodings::RGB8); + if (!source_image_measurement) return; + auto& source_image = source_image_measurement->image; + auto target_image_measurement = lm::MakeImageMeasurement(*target_image_msg, sensor_msgs::image_encodings::RGB8); + if (!target_image_measurement) return; + auto& target_image = target_image_measurement->image; + + // Create correspondence image + // Draw source image above target image, add correspondences as points outlined + // by rectangles to each image + cv_bridge::CvImage single_correspondence_image; + single_correspondence_image.encoding = sensor_msgs::image_encodings::RGB8; + const int rows = target_image.rows; + const int cols = target_image.cols; + single_correspondence_image.image = cv::Mat(rows * 2, cols, CV_8UC3, cv::Scalar(0, 0, 0)); + + const cv::Point2f source_image_point = + CvPoint(depth_odometry.correspondences.source_image_points[correspondence_index]); + const Eigen::Vector3d frame_changed_target_3d_point = + depth_odometry.odometry.sensor_F_source_T_target.pose * target_3d_point; + const auto projected_target_point = projectPoint(frame_changed_target_3d_point); + const cv::Point2f target_image_point = + CvPoint(depth_odometry.correspondences.target_image_points[correspondence_index]); + const cv::Point2f rectangle_offset(40, 40); + cv::circle(source_image, source_image_point, 5 /* Radius*/, cv::Scalar(0, 255, 0), -1 /*Filled*/, 8); + cv::rectangle(source_image, source_image_point - rectangle_offset, source_image_point + rectangle_offset, + cv::Scalar(0, 255, 0), 8); + cv::circle(source_image, projected_target_point, 3 /* Radius*/, cv::Scalar(255, 0, 0), -1 /*Filled*/, 8); + cv::circle(target_image, target_image_point, 5 /* Radius*/, cv::Scalar(0, 255, 0), -1 /*Filled*/, 8); + cv::rectangle(target_image, target_image_point - rectangle_offset, target_image_point + rectangle_offset, + cv::Scalar(0, 255, 0), 8); + source_image.copyTo(single_correspondence_image.image(cv::Rect(0, 0, cols, rows))); + target_image.copyTo(single_correspondence_image.image(cv::Rect(0, rows, cols, rows))); + single_correspondence_image_pub_.publish(single_correspondence_image.toImageMsg()); +} + +void DepthOdometryDisplay::publishCorrespondencePoints(const Eigen::Vector3d& source_3d_point, + const Eigen::Vector3d& target_3d_point, + const lc::Time source_time, const lc::Time target_time) { + const auto source_point_cloud = point_cloud_buffer_.GetNearby(source_time, 0.05); + const auto target_point_cloud = point_cloud_buffer_.GetNearby(target_time, 0.05); + if (!source_point_cloud || !target_point_cloud) return; + geometry_msgs::PointStamped source_3d_point_msg; + mc::VectorToMsg(source_3d_point, source_3d_point_msg.point); + source_3d_point_msg.header.stamp = ros::Time::now(); + source_3d_point_msg.header.frame_id = "haz_cam"; + source_correspondence_point_pub_.publish(source_3d_point_msg); + + geometry_msgs::PointStamped target_3d_point_msg; + mc::VectorToMsg(target_3d_point, target_3d_point_msg.point); + target_3d_point_msg.header.stamp = ros::Time::now(); + target_3d_point_msg.header.frame_id = "haz_cam"; + target_correspondence_point_pub_.publish(target_3d_point_msg); + + { + const auto source_cloud_msg = + lm::MakePointCloudMsg(**source_point_cloud, lc::TimeFromRosTime(ros::Time::now()), "haz_cam"); + source_point_cloud_pub_.publish(source_cloud_msg); + const auto target_cloud_msg = + lm::MakePointCloudMsg(**target_point_cloud, lc::TimeFromRosTime(ros::Time::now()), "haz_cam"); + target_point_cloud_pub_.publish(target_cloud_msg); + } +} +} // namespace localization_rviz_plugins + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(localization_rviz_plugins::DepthOdometryDisplay, rviz::Display) diff --git a/tools/localization_rviz_plugins/src/depth_odometry_display.h b/tools/localization_rviz_plugins/src/depth_odometry_display.h new file mode 100644 index 0000000000..e16c437c28 --- /dev/null +++ b/tools/localization_rviz_plugins/src/depth_odometry_display.h @@ -0,0 +1,89 @@ +/* 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 file must go in src directory for Qt/Rviz plugin +#ifndef LOCALIZATION_RVIZ_PLUGINS_DEPTH_ODOMETRY_DISPLAY_H_ // NOLINT +#define LOCALIZATION_RVIZ_PLUGINS_DEPTH_ODOMETRY_DISPLAY_H_ // NOLINT + +// Required for Qt +#ifndef Q_MOC_RUN +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "slider_property.h" // NOLINT +#endif + +// Forward declarations for ogre and rviz +namespace Ogre { +class SceneNode; +} + +namespace localization_rviz_plugins { +class DepthOdometryDisplay : public rviz::MessageFilterDisplay { + Q_OBJECT // NOLINT + public : // NOLINT + DepthOdometryDisplay(); + ~DepthOdometryDisplay() = default; + + // private: + protected: + void onInitialize() final; + void reset() final; + + private Q_SLOTS: // NOLINT + void createSingleCorrespondenceDisplays(); + + private: + void processMessage(const ff_msgs::DepthOdometry::ConstPtr& depth_odometry_msg); + void createProjectionAndOpticalFlowImage(const localization_measurements::DepthOdometryMeasurement& depth_odometry); + cv::Point2f projectPoint(const Eigen::Vector3d& point_3d); + void imageCallback(const sensor_msgs::ImageConstPtr& image_msg); + void pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& point_cloud_msg); + void publishCorrespondencePoints(const Eigen::Vector3d& source_3d_point, const Eigen::Vector3d& target_3d_point, + const localization_common::Time source_time, + const localization_common::Time target_time); + void clearDisplay(); + + std::unique_ptr camera_params_; + std::unique_ptr correspondence_index_slider_; + cv::Mat intrinsics_; + cv::Mat distortion_params_; + image_transport::Subscriber image_sub_; + ros::Subscriber point_cloud_sub_; + ros::Publisher source_correspondence_point_pub_, target_correspondence_point_pub_; + ros::Publisher source_point_cloud_pub_, target_point_cloud_pub_; + image_transport::Publisher single_correspondence_image_pub_, projection_and_optical_flow_image_pub_; + ros::NodeHandle nh_; + localization_common::MeasurementBuffer img_buffer_; + localization_common::MeasurementBuffer::Ptr> point_cloud_buffer_; + boost::optional latest_depth_odometry_measurement_; +}; +} // namespace localization_rviz_plugins +#endif // LOCALIZATION_RVIZ_PLUGINS_DEPTH_ODOMETRY_DISPLAY_H_ NOLINT diff --git a/tools/localization_rviz_plugins/src/localization_graph_display.cc b/tools/localization_rviz_plugins/src/localization_graph_display.cc index c8e10a752d..a10c918654 100644 --- a/tools/localization_rviz_plugins/src/localization_graph_display.cc +++ b/tools/localization_rviz_plugins/src/localization_graph_display.cc @@ -200,7 +200,7 @@ void LocalizationGraphDisplay::addProjectionVisual(const gtsam::CameraSet + performance_tester + 1.0.0 + + Used to test robots performance + + + Apache License, Version 2.0 + + + Astrobee Flight Software + + + Astrobee Flight Software + + catkin + eigen_conversions + roscpp + rosbag + ff_util + ff_msgs + ff_common + roscpp + rosbag + ff_util + ff_msgs + ff_common + diff --git a/tools/performance_tester/readme.md b/tools/performance_tester/readme.md new file mode 100644 index 0000000000..59a27197d6 --- /dev/null +++ b/tools/performance_tester/readme.md @@ -0,0 +1,25 @@ +\page performance_tester Performance Tester + +## Tools + +### Test Plan + +This tool can be used to test a plan execution. It executes a startup fplan, starts recording, +and starts the plan execution. The recording stops when the plan is completed. This is useful for +granite lab testing. +To execute the tool, run: + + rosrun performance_tester test_plan -startup_plan $STARTUP_FPLAN -data_to_disk $DATA_PROFILE -move_plan $MOVE_FPLAN + +## Scripts + +### Analyse bag + +This script analyses bag files and plots several output plots for analysis. +It analyzes the CPU and Memory used, as well as the rate of publishing of topics. +To run the script for a certain bag file: + + ./analyse_bag.py --bag-name $BAG_NAME + +By default the topics that is scoped for frequency analysis are: "/loc/ml/features", "/hw/imu", "/gnc/ekf", "/gnc/ctl/command". +To customize this, the argument '--topic-list' can be defined when executing the script to specify the topics. \ No newline at end of file diff --git a/tools/performance_tester/scripts/analyse_bag.py b/tools/performance_tester/scripts/analyse_bag.py new file mode 100755 index 0000000000..517ea585cc --- /dev/null +++ b/tools/performance_tester/scripts/analyse_bag.py @@ -0,0 +1,379 @@ +#!/usr/bin/env python +# 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. + +import argparse +import os + +import matplotlib.pyplot as plt +import numpy as np +import rosbag + +EPS = 1e-8 +cpu_topic = "/mgt/cpu_monitor/state" +default_topics = ["/loc/ml/features", "/hw/imu", "/gnc/ekf", "/gnc/ctl/command"] + + +class Topic: + def __init__(self, name, n, n_total): + self.n_topic_total = n_total + self.n_topic = n + self.name = name + self.hz_max = 0 + self.hz_min = 100 + self.time = [] + self.frequency = [] + self.t_diff = [] + self.t_last = -1 + + def addTimestamp(self, t): + # Check if it is the fist available measurement + if self.t_last == -1: + self.t_last = t + return + + # Check if we have repeated timestamps + if t - self.t_last < EPS: + print("same pub time") + return + + hz = 1 / (t - self.t_last) + self.frequency.append(hz) + self.t_diff.append(t - self.t_last) + self.time.append(t) + + if hz > self.hz_max: + self.hz_max = hz + if hz < self.hz_min: + self.hz_min = hz + + # Update parameters + self.t_last = t + + def printResult(self, i, ax): + # Plot the frequency over time + xpoints = np.array(self.time) - self.time[0] + ypoints = np.array(self.frequency) + + ax[i, 0].plot(xpoints, ypoints) + ax[i, 0].set_xlabel("Time(s)") + ax[i, 0].set_ylabel("Frequency (Hz)") + ax[i, 0].set_title(self.name) + + # Print the histogram with percentage y-axis + num_bins = 100 + heights, bins = np.histogram(self.t_diff, num_bins) + percent = [float(j) / sum(heights) * 100 for j in heights] + ax[i, 1].bar(bins[:-1], percent, width=bins[1] - bins[0], align="edge") + ax[i, 1].set_xlabel("Time(s)") + ax[i, 1].set_ylabel("Percentage(%)") + ax[i, 1].set_title(self.name + " Time between messages histogram") + + @classmethod + def initDictFromTopics(cls, topic_list): + return dict( + (topic, cls(topic, i, len(topic_list))) + for i, topic in enumerate(topic_list) + ) + + @classmethod + def plotResults(cls, topics): + # Start figure + fig, ax = plt.subplots(len(topics), 2, figsize=(16, 16)) + plt.subplots_adjust( + left=None, bottom=None, right=None, top=None, wspace=None, hspace=0.35 + ) + for i, key in enumerate(topics.keys()): + topics[key].printResult(i - 1, ax) + + plt.show() + + +class CpuUsage: + def __init__(self): + self.cpu_topic = "/mgt/cpu_monitor/state" + + # Total CPU usage + self.mlp_cpu_loads = [[], [], [], []] + self.mlp_time = [] + self.llp_cpu_loads = [[], []] + self.llp_time = [] + + # Node CPU usage + self.llp_nodes = {} + self.llp_nodes_time = [] + self.mlp_nodes = {} + self.mlp_nodes_time = [] + + def addCpuMessage(self, topic, msg): + # Check if the topic is the one we're interested in + if topic != self.cpu_topic: + return + + # We want the total cpu usage + total_cpu = 0 + for field in msg.load_fields: + if field == msg.TOTAL: + break + total_cpu = total_cpu + 1 + + if msg.name == "llp": + self.llp_time.append(msg.header.stamp.to_sec()) + for i in range(len(msg.cpus)): + self.llp_cpu_loads[i].append(msg.cpus[i].loads[total_cpu]) + + # Save nodes info + append_time_llp = False + for node in msg.load_nodes: + append_time_llp = True + if node.name in self.llp_nodes: + self.llp_nodes[node.name].append(node.load) + else: + self.llp_nodes[node.name] = [node.load] + if append_time_llp: + self.llp_nodes_time.append(msg.header.stamp.to_sec()) + + if msg.name == "mlp": + self.mlp_time.append(msg.header.stamp.to_sec()) + for i in range(len(msg.cpus)): + self.mlp_cpu_loads[i].append(msg.cpus[i].loads[total_cpu]) + + # Save nodes info + append_time_mlp = False + for node in msg.load_nodes: + append_time_mlp = True + if node.name in self.mlp_nodes: + self.mlp_nodes[node.name].append(node.load) + else: + self.mlp_nodes[node.name] = [node.load] + if append_time_mlp: + self.mlp_nodes_time.append(msg.header.stamp.to_sec()) + + def printResult(self): + fig, ax = plt.subplots(3, 2, figsize=(16, 16)) + + # Plot LLP CPU usage + xpoints = np.array(self.llp_time) - self.llp_time[0] + for cpu in range(len(self.llp_cpu_loads)): + ypoints = np.array(self.llp_cpu_loads[cpu]) + ax[0, 0].plot(xpoints, ypoints, label="cpu" + str(cpu)) + ax[0, 0].set_xlabel("Time(s)") + ax[0, 0].set_ylabel("Load") + ax[0, 0].set_title("LLP CPU loads") + ax[0, 0].legend() + ax[0, 0].set_ylim([0, 100]) + + # Plot MLP CPU usage + xpoints = np.array(self.mlp_time) - self.llp_time[0] + for cpu in range(len(self.mlp_cpu_loads)): + ypoints = np.array(self.mlp_cpu_loads[cpu]) + ax[0, 1].plot(xpoints, ypoints, label="cpu" + str(cpu)) + ax[0, 1].set_xlabel("Time(s)") + ax[0, 1].set_ylabel("Load") + ax[0, 1].set_title("MLP CPU loads") + ax[0, 1].legend() + ax[0, 1].set_ylim([0, 100]) + + # Plot nodes usage + xpoints = np.array(self.llp_nodes_time) - self.llp_nodes_time[0] + for llp_node in self.llp_nodes: + ax[1, 0].plot(xpoints, self.llp_nodes[llp_node], label=llp_node, alpha=0.8) + ax[1, 0].set_xlabel("Time(s)") + ax[1, 0].set_ylabel("Load") + ax[1, 0].set_title("Node CPU loads") + ax[1, 0].legend() + + xpoints = np.array(self.mlp_nodes_time) - self.mlp_nodes_time[0] + for mlp_node in self.mlp_nodes: + ax[1, 1].plot(xpoints, self.mlp_nodes[mlp_node], label=mlp_node, alpha=0.8) + ax[1, 1].set_xlabel("Time(s)") + ax[1, 1].set_ylabel("Load") + ax[1, 1].set_title("Node CPU loads") + ax[1, 1].legend() + + # Plot nodes usage cummulative + ax[2, 0].stackplot( + np.array(self.llp_nodes_time) - self.llp_nodes_time[0], + self.llp_nodes.values(), + labels=self.llp_nodes.keys(), + alpha=0.8, + ) + ax[2, 0].set_xlabel("Time(s)") + ax[2, 0].set_ylabel("Load") + ax[2, 0].set_title("Node CPU commulative loads") + ax[2, 0].legend() + + ax[2, 1].stackplot( + np.array(self.mlp_nodes_time) - self.mlp_nodes_time[0], + self.mlp_nodes.values(), + labels=self.mlp_nodes.keys(), + alpha=0.8, + ) + ax[2, 1].set_xlabel("Time(s)") + ax[2, 1].set_ylabel("Load") + ax[2, 1].set_title("Node CPU commulative loads") + ax[2, 1].legend() + + +class MemUsage: + def __init__(self): + self.mem_topic = "/mgt/mem_monitor/state" + # Analyse total memory usage + self.llp_mem_load = [] + self.llp_time = [] + self.mlp_mem_load = [] + self.mlp_time = [] + # Analyse individual node memory usage + self.llp_nodes = {} + self.llp_nodes_time = [] + self.mlp_nodes = {} + self.mlp_nodes_time = [] + + self.llp_init = False + self.mlp_init = False + + def addMemMessage(self, topic, msg): + # Check if the topic is the one we're interested in + if topic != self.mem_topic: + return + + # Save ram values + if msg.name == "llp": + if self.llp_init == False: + self.llp_ram_total = msg.ram_total + self.llp_mem_load.append(msg.ram_used) + self.llp_time.append(msg.header.stamp.to_sec()) + + append_time_llp = False + for node in msg.nodes: + append_time_llp = True + if node.name in self.llp_nodes: + self.llp_nodes[node.name].append(node.ram_perc) + else: + self.llp_nodes[node.name] = [node.ram_perc] + if append_time_llp: + self.llp_nodes_time.append(msg.header.stamp.to_sec()) + + if msg.name == "mlp": + if self.mlp_init == False: + self.mlp_ram_total = msg.ram_total + self.mlp_mem_load.append(msg.ram_used) + self.mlp_time.append(msg.header.stamp.to_sec()) + + append_time_mlp = False + for node in msg.nodes: + append_time_mlp = True + if node.name in self.mlp_nodes: + self.mlp_nodes[node.name].append(node.ram_perc) + else: + self.mlp_nodes[node.name] = [node.ram_perc] + if append_time_mlp: + self.mlp_nodes_time.append(msg.header.stamp.to_sec()) + + def printResult(self): + fig, ax = plt.subplots(2, 2, figsize=(16, 16)) + + # Plot LLP CPU usage + xpoints = np.array(self.llp_time) - self.llp_time[0] + ypoints = 100 * np.array(self.llp_mem_load) / self.llp_ram_total + ax[0, 0].plot(xpoints, ypoints) + ax[0, 0].set_xlabel("Time(s)") + ax[0, 0].set_ylabel("Load") + ax[0, 0].set_title("LLP Memory RAM loads") + # ax[0, 0].legend() + ax[0, 0].set_ylim([0, 100]) + + # Plot MLP CPU usage + xpoints = np.array(self.mlp_time) - self.llp_time[0] + ypoints = 100 * np.array(self.mlp_mem_load) / self.mlp_ram_total + ax[0, 1].plot(xpoints, ypoints) + ax[0, 1].set_xlabel("Time(s)") + ax[0, 1].set_ylabel("Load") + ax[0, 1].set_title("MLP Memory RAM loads") + # ax[0, 1].legend() + ax[0, 1].set_ylim([0, 100]) + + # Plot nodes usage + # xpoints = np.array(self.llp_nodes_time) - self.llp_nodes_time[0] + # for llp_node in self.llp_nodes: + # ax[1, 0].plot(xpoints, self.llp_nodes[llp_node], label=llp_node, alpha=0.8) + # ax[1, 0].set_xlabel("Time(s)") + # ax[1, 0].set_ylabel("Load") + # ax[1, 0].set_title("Node Memory RAM loads") + # ax[1, 0].legend() + + # xpoints = np.array(self.mlp_nodes_time) - self.mlp_nodes_time[0] + # for mlp_node in self.mlp_nodes: + # ax[1, 1].plot(xpoints, self.mlp_nodes[mlp_node], label=mlp_node, alpha=0.8) + # ax[1, 1].set_xlabel("Time(s)") + # ax[1, 1].set_ylabel("Load") + # ax[1, 1].set_title("Node Memory RAM loads") + # ax[1, 1].legend() + + # Plot nodes usage cummulative + ax[1, 0].stackplot( + np.array(self.llp_nodes_time) - self.llp_nodes_time[0], + self.llp_nodes.values(), + labels=self.llp_nodes.keys(), + alpha=0.8, + ) + ax[1, 0].set_xlabel("Time(s)") + ax[1, 0].set_ylabel("Load") + ax[1, 0].set_title("Node Memory RAM commulative loads") + ax[1, 0].legend() + + ax[1, 1].stackplot( + np.array(self.mlp_nodes_time) - self.mlp_nodes_time[0], + self.mlp_nodes.values(), + labels=self.mlp_nodes.keys(), + alpha=0.8, + ) + ax[1, 1].set_xlabel("Time(s)") + ax[1, 1].set_ylabel("Load") + ax[1, 1].set_title("Node CPU commulative loads") + ax[1, 1].legend() + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("--bag-name", default="") + parser.add_argument("-n", "--topic-list", nargs="+", default=default_topics) + + args = parser.parse_args() + + # Create topic dictionary + freq_analyzer = Topic.initDictFromTopics(args.topic_list) + # Analyse resource loads + cpu_analyzer = CpuUsage() + mem_analyzer = MemUsage() + + # Go through the messages in the bag file + for topic_bag, msg, t in rosbag.Bag(args.bag_name).read_messages(): + # Add message to cpu analyser + cpu_analyzer.addCpuMessage(topic_bag, msg) + # Add message to mem analyser + mem_analyzer.addMemMessage(topic_bag, msg) + # Check if the topic matches + for topic in args.topic_list: + if topic_bag == topic: + # Calculate hz + freq_analyzer[topic].addTimestamp(msg.header.stamp.to_sec()) + + # Plot the result + cpu_analyzer.printResult() + mem_analyzer.printResult() + Topic.plotResults(freq_analyzer) diff --git a/tools/performance_tester/tools/test_plan.cc b/tools/performance_tester/tools/test_plan.cc new file mode 100644 index 0000000000..fc92128d9e --- /dev/null +++ b/tools/performance_tester/tools/test_plan.cc @@ -0,0 +1,106 @@ +/* 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. + */ + +// Command line flags +#include +#include + +// Include RPOS +#include + +// FSW includes +#include +#include +#include + +// Software messages +#include +#include +#include + +// C++ STL includes +#include +#include +#include +#include +#include +#include + +// Gflags +DEFINE_string(ns, "", "Robot namespace"); + +DEFINE_string(startup_plan, "", "Input file with rows of type: x y z roll pitch yaw (in degrees)."); +DEFINE_string(data_to_disk, "fplan", "ll"); +DEFINE_string(move_plan, "", "Output file."); + +ros::ServiceClient client_; + + +// Wait for plan to finish +void PlanCallback(ff_msgs::PlanStatusStamped::ConstPtr const& ps) { + if (ps->status.status == ff_msgs::AckStatus::COMPLETED) { + // Stop recording + ff_msgs::EnableRecording srv; + srv.request.enable = false; + if (client_.call(srv)) + ROS_ERROR_STREAM("Result: " << srv.response.success); + ros::shutdown(); + } +} + +// Main entry point for application +int main(int argc, char *argv[]) { + // Initialize a ros node + ros::init(argc, argv, "test_plan", ros::init_options::AnonymousName); + // Gather some data from the command + google::SetUsageMessage("Usage: rosrun performance_tester test_plan "); + google::SetVersionString("1.0.0"); + google::ParseCommandLineFlags(&argc, &argv, true); + // Create a node handle + ros::NodeHandle nh(std::string("/") + FLAGS_ns); + // Start bagger service client + client_ = nh.serviceClient(SERVICE_MANAGEMENT_DATA_BAGGER_ENABLE_RECORDING); + + + // Run Startup plan + std::system(("rosrun executive plan_pub " + FLAGS_startup_plan).c_str()); + + // Configure recording data + std::system(("rosrun executive data_to_disk_pub " + FLAGS_data_to_disk).c_str()); + + // Start recording data + ff_msgs::EnableRecording srv; + srv.request.enable = true; + if (client_.call(srv)) + ROS_ERROR_STREAM("Result: " << srv.response.success); + + // Start movement plan + std::system(("rosrun executive plan_pub " + FLAGS_move_plan).c_str()); + + + // Monitor plan + ros::Subscriber sub = nh.subscribe( + TOPIC_MANAGEMENT_EXEC_PLAN_STATUS, 1, &PlanCallback); + + // Synchronous mode + ros::spin(); + // Finish commandline flags + google::ShutDownCommandLineFlags(); + // Make for great success + return 0; +} diff --git a/tools/readme.md b/tools/readme.md index 0ce4200c7b..48bcd619f0 100644 --- a/tools/readme.md +++ b/tools/readme.md @@ -3,6 +3,8 @@ This directory contains tools that are useful for debugging, but are not run on the robot. +\subpage calibration + \subpage ekfbag \subpage ekfvideo @@ -22,3 +24,7 @@ but are not run on the robot. \subpage visualeyez \subpage dds_profile + +\subpage interactive_marker_teleop + +\subpage performance_tester \ No newline at end of file