Split Localizer into GraphVIO and GraphLocalizer (#724)

Added new split localizer with time-based state estimation library and depth odometry integration.

astrobee/config/gnc.config

@@ -18,6 +18,8 @@
 require "common_lua"
 require "geometry"
 
+-- Velocity Feedforward Gain
+tun_vel_gain = 0.5;
 -- Accel Feedforward Gains
 tun_accel_gain = vec3(1.0, 1.0, 1.0);
 tun_alpha_gain = vec3(1.0, 1.0, 1.0);

astrobee/config/localization/depth_odometry.config

@@ -32,6 +32,13 @@ orientation_covariance_threshold = 100
 -- icp or image_feature
 depth_odometry_method = "image_feature"
 
+-- Buffer size for images and point clouds
+max_buffer_size = 5
+
+-- Max number of depth images to create in a single iteration
+-- Limits depth odometry runtime 
+max_depth_images = 2
+
 -- ICP options
 
 -- Search radius for each point in ICP 
@@ -122,9 +129,18 @@ clahe_clip_limit = 40
 -- Other
 min_x_distance_to_border = 10 
 min_y_distance_to_border = 10
-min_num_inliers = 5
+min_num_correspondences = 5
+-- Don't compute relative pose and just return 3D point correspondences
+-- Only applicable for image_feature approach 
+only_correspondences = true 
 -- Refine estimate with PointToPlaneICP
 refine_estimate = true
+-- Filter outliers using essential matrix estimation
+filter_outliers = false 
+-- ransac, lmeds, rho (rho is actual prosac)
+filter_method = "rho" 
+inlier_threshold = 1.0
+inlier_probability = 0.999
 
 -- Point cloud with known correspondences aligner
 pcwkca_max_num_iterations = 100

astrobee/config/localization/graph_localizer.config

@@ -0,0 +1,82 @@
+-- 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"
+
+-- Graph Localizer (gl)
+-- Factor Adders (fa)
+--- Loc 
+---- Sparse Map (sm)
+gl_fa_loc_sm_enabled = true
+gl_fa_loc_sm_huber_k = world_huber_k
+gl_fa_loc_sm_add_pose_priors = false 
+gl_fa_loc_sm_add_projection_factors = true
+gl_fa_loc_sm_add_prior_if_projection_factors_fail = true 
+gl_fa_loc_sm_prior_translation_stddev = 0.06 
+gl_fa_loc_sm_prior_quaternion_stddev = 0.06
+gl_fa_loc_sm_scale_pose_noise_with_num_landmarks = false 
+gl_fa_loc_sm_scale_projection_noise_with_num_landmarks = false 
+gl_fa_loc_sm_scale_projection_noise_with_landmark_distance = false 
+--- change to <=1 w/ new vio cov
+gl_fa_loc_sm_pose_noise_scale = 0.001 
+gl_fa_loc_sm_projection_noise_scale = 1.5 
+gl_fa_loc_sm_max_num_projection_factors = 25 
+gl_fa_loc_sm_min_num_matches_per_measurement = 5 
+gl_fa_loc_sm_max_valid_projection_error = 30
+gl_fa_loc_sm_nav_cam_noise_stddev = 0.1
+---- AR Tag (ar)
+gl_fa_loc_ar_enabled = true 
+gl_fa_loc_ar_huber_k = world_huber_k
+gl_fa_loc_ar_add_pose_priors = false 
+gl_fa_loc_ar_add_projection_factors = true
+gl_fa_loc_ar_add_prior_if_projection_factors_fail = true 
+gl_fa_loc_ar_prior_translation_stddev = 0.06 
+gl_fa_loc_ar_prior_quaternion_stddev = 0.06
+gl_fa_loc_ar_scale_pose_noise_with_num_landmarks = false 
+gl_fa_loc_ar_scale_projection_noise_with_num_landmarks = false 
+gl_fa_loc_ar_scale_projection_noise_with_landmark_distance = false 
+gl_fa_loc_ar_pose_noise_scale = 0.01 
+-- Change to 9 if scale with inverse distance
+gl_fa_loc_ar_projection_noise_scale = 10 
+gl_fa_loc_ar_max_num_projection_factors = 25 
+gl_fa_loc_ar_min_num_matches_per_measurement = 5 
+gl_fa_loc_ar_max_valid_projection_error = 30
+gl_fa_loc_ar_dock_cam_noise_stddev = 0.1
+-- Node Adders (na)
+--- Pose
+gl_na_pose_starting_prior_translation_stddev = 0.02 
+gl_na_pose_starting_prior_quaternion_stddev = 0.01
+gl_na_pose_huber_k = world_huber_k 
+gl_na_pose_add_priors = true 
+gl_na_pose_ideal_duration = 4 
+gl_na_pose_min_num_states = 3 
+gl_na_pose_max_num_states = 5
+gl_na_pose_model_huber_k = world_huber_k
+-- Optimizer (op)
+--- Nonlinear
+gl_op_nl_marginals_factorization = "qr"
+gl_op_nl_max_iterations = 10 
+gl_op_nl_verbose = false 
+gl_op_nl_use_ceres_params = false 
+-- Graph Optimizer (go)
+--- Sliding Window 
+gl_go_sw_huber_k = world_huber_k 
+gl_go_sw_log_stats_on_destruction = false
+gl_go_sw_print_after_optimization = false
+gl_go_sw_add_marginal_factors = false 
+gl_go_sw_slide_window_before_optimization = true
+-- Other
+gl_max_vio_measurement_gap = 3