From 8b10c89a6649d0bfc5c0494f87be5473c27dc078 Mon Sep 17 00:00:00 2001 From: Amanda Adkins Date: Tue, 5 Dec 2023 16:31:12 -0600 Subject: [PATCH] Add a couple things in visual feature front-end to config; move final refinement after online portion --- config/revision_base.json | 467 ++++++++++++++++++ .../cv_file_storage/config_file_storage_io.h | 47 +- .../configuration/full_ov_slam_config.h | 10 +- .../offline/offline_problem_runner.h | 18 +- .../visual_feature_front_end.h | 17 +- .../optimization_runner.h | 4 +- .../configuration/write_configuration.cpp | 6 +- .../config_file_storage_io_tests.cc | 2 + 8 files changed, 545 insertions(+), 26 deletions(-) create mode 100644 config/revision_base.json diff --git a/config/revision_base.json b/config/revision_base.json new file mode 100644 index 00000000..5a846a51 --- /dev/null +++ b/config/revision_base.json @@ -0,0 +1,467 @@ +{ + "config": { + "config_schema_version": 13, + "config_version_id": "revision_base", + "visual_feature_params": { + "reprojection_error_std_dev": 1.0, + "min_visual_feature_parallax_pixel_requirement": 5.0, + "min_visual_feature_parallax_robot_transl_requirement": 1.0000000000000001e-01, + "min_visual_feature_parallax_robot_orient_requirement": 5.0000000000000003e-02, + "enforce_min_pixel_parallax_requirement": 1, + "enforce_min_robot_pose_parallax_requirement": 0, + "inlier_epipolar_err_thresh": 8.0, + "check_past_n_frames_for_epipolar_err": 5, + "enforce_epipolar_error_requirement_": 1, + "early_votes_return": 1, + "visual_feature_inlier_majority_percentage": 5.0000000000000000e-01 + }, + "local_ba_iteration_params": { + "allow_reversion_after_detecting_jumps": 1, + "consecutive_pose_transl_tol": 1.0, + "consecutive_pose_orient_tol": 3.1415926535897931e+00, + "feature_outlier_percentage": 1.0000000000000001e-01, + "phase_one_opt_params": { + "max_num_iterations": 50, + "allow_non_monotonic_steps": 1, + "function_tolerance": 9.9999999999999995e-04, + "gradient_tolerance": 1.0000000000000000e-10, + "parameter_tolerance": 1.0000000000000000e-08, + "initial_trust_region_radius": 100.0, + "max_trust_region_radius": 10000.0 + }, + "phase_two_opt_params": { + "max_num_iterations": 100, + "allow_non_monotonic_steps": 1, + "function_tolerance": 9.9999999999999995e-05, + "gradient_tolerance": 1.0000000000000000e-10, + "parameter_tolerance": 1.0000000000000000e-08, + "initial_trust_region_radius": 100.0, + "max_trust_region_radius": 10000.0 + } + }, + "global_ba_iteration_params": { + "allow_reversion_after_detecting_jumps": 1, + "consecutive_pose_transl_tol": 1.0, + "consecutive_pose_orient_tol": 3.1415926535897931e+00, + "feature_outlier_percentage": 1.0000000000000001e-01, + "phase_one_opt_params": { + "max_num_iterations": 250, + "allow_non_monotonic_steps": 1, + "function_tolerance": 9.9999999999999995e-07, + "gradient_tolerance": 1.0000000000000000e-10, + "parameter_tolerance": 1.0000000000000000e-08, + "initial_trust_region_radius": 100.0, + "max_trust_region_radius": 10000.0 + }, + "phase_two_opt_params": { + "max_num_iterations": 250, + "allow_non_monotonic_steps": 1, + "function_tolerance": 9.9999999999999995e-07, + "gradient_tolerance": 1.0000000000000000e-10, + "parameter_tolerance": 1.0000000000000000e-08, + "initial_trust_region_radius": 100.0, + "max_trust_region_radius": 10000.0 + } + }, + "final_ba_iteration_params": { + "allow_reversion_after_detecting_jumps": 1, + "consecutive_pose_transl_tol": 1.0, + "consecutive_pose_orient_tol": 3.1415926535897931e+00, + "feature_outlier_percentage": 1.0000000000000001e-01, + "phase_one_opt_params": { + "max_num_iterations": 300, + "allow_non_monotonic_steps": 1, + "function_tolerance": 9.9999999999999995e-07, + "gradient_tolerance": 1.0000000000000000e-10, + "parameter_tolerance": 1.0000000000000000e-08, + "initial_trust_region_radius": 100.0, + "max_trust_region_radius": 10000.0 + }, + "phase_two_opt_params": { + "max_num_iterations": 300, + "allow_non_monotonic_steps": 1, + "function_tolerance": 9.9999999999999995e-07, + "gradient_tolerance": 1.0000000000000000e-10, + "parameter_tolerance": 1.0000000000000000e-08, + "initial_trust_region_radius": 100.0, + "max_trust_region_radius": 10000.0 + } + }, + "pgo_solver_params": { + "relative_pose_factor_huber_loss": 5.0, + "enable_visual_feats_only_opt_post_pgo": 1, + "enable_visual_non_opt_feature_adjustment_post_pgo": 1, + "relative_pose_cov_params": { + "transl_error_mult_for_transl_error": 1.0000000000000001e-01, + "transl_error_mult_for_rot_error": 1.0000000000000001e-01, + "rot_error_mult_for_transl_error": 5.0000000000000001e-01, + "rot_error_mult_for_rot_error": 5.0000000000000001e-01 + }, + "pgo_optimization_solver_params": { + "max_num_iterations": 250, + "allow_non_monotonic_steps": 1, + "function_tolerance": 9.9999999999999995e-07, + "gradient_tolerance": 1.0000000000000000e-10, + "parameter_tolerance": 1.0000000000000000e-08, + "initial_trust_region_radius": 100.0, + "max_trust_region_radius": 10000.0 + }, + "final_pgo_optimization_solver_params": { + "max_num_iterations": 300, + "allow_non_monotonic_steps": 1, + "function_tolerance": 9.9999999999999995e-07, + "gradient_tolerance": 1.0000000000000000e-10, + "parameter_tolerance": 1.0000000000000000e-08, + "initial_trust_region_radius": 100.0, + "max_trust_region_radius": 10000.0 + } + }, + "ltm_tunable_params": { + "far_feature_threshold": 75.0, + "min_col_norm": 5.0000000000000001e-04, + "fallback_to_prev_for_failed_extraction": 1 + }, + "ltm_solver_residual_params": { + "object_residual_params": { + "object_observation_huber_loss_param": 5.0000000000000000e-01, + "shape_dim_prior_factor_huber_loss_param": 10.0, + "invalid_ellipsoid_error_val": 1000.0 + }, + "visual_residual_params": { + "reprojection_error_huber_loss_param": 1.0 + }, + "long_term_map_params": { + "pair_huber_loss_param": 1.0 + }, + "relative_pose_factor_huber_loss": 1.0, + "relative_pose_cov_params": { + "transl_error_mult_for_transl_error": 0.0, + "transl_error_mult_for_rot_error": 0.0, + "rot_error_mult_for_transl_error": 6.9245860980687362e-310, + "rot_error_mult_for_rot_error": -4.4579057385799811e+204 + } + }, + "ltm_solver_params": { + "max_num_iterations": 300, + "allow_non_monotonic_steps": 1, + "function_tolerance": 9.9999999999999995e-07, + "gradient_tolerance": 1.0000000000000000e-10, + "parameter_tolerance": 1.0000000000000000e-08, + "initial_trust_region_radius": 10000.0, + "max_trust_region_radius": 1.0000000000000000e+16 + }, + "shape_dimension_priors": { + "dimension_prior_label": [ + { + "semantic_class": "chair", + "obj_dim_mean": { + "Rows": 3, + "Cols": 1, + "Data": [ + 6.2000000000000000e-01, + 6.2000000000000000e-01, + 9.7499999999999998e-01 + ] + }, + "dim_covariance": { + "Rows": 3, + "Cols": 3, + "Data": [ + 2.5000000000000005e-03, + 0.0, + 0.0, + 0.0, + 2.5000000000000005e-03, + 0.0, + 0.0, + 0.0, + 2.5000000000000005e-03 + ] + } + }, + { + "semantic_class": "bench", + "obj_dim_mean": { + "Rows": 3, + "Cols": 1, + "Data": [ + 1.0, + 2.5000000000000000e+00, + 1.5000000000000000e+00 + ] + }, + "dim_covariance": { + "Rows": 3, + "Cols": 3, + "Data": [ + 2.2500000000000000e+00, + 0.0, + 0.0, + 0.0, + 4.0, + 0.0, + 0.0, + 0.0, + 2.2500000000000000e+00 + ] + } + }, + { + "semantic_class": "roadblock", + "obj_dim_mean": { + "Rows": 3, + "Cols": 1, + "Data": [ + 2.8999999999999998e-01, + 2.8999999999999998e-01, + 4.7999999999999998e-01 + ] + }, + "dim_covariance": { + "Rows": 3, + "Cols": 3, + "Data": [ + 9.9999999999999995e-07, + 0.0, + 0.0, + 0.0, + 9.9999999999999995e-07, + 0.0, + 0.0, + 0.0, + 1.0000000000000000e-04 + ] + } + }, + { + "semantic_class": "treetrunk", + "obj_dim_mean": { + "Rows": 3, + "Cols": 1, + "Data": [ + 4.0000000000000002e-01, + 4.0000000000000002e-01, + 2.0 + ] + }, + "dim_covariance": { + "Rows": 3, + "Cols": 3, + "Data": [ + 4.0000000000000008e-02, + 0.0, + 0.0, + 0.0, + 4.0000000000000008e-02, + 0.0, + 0.0, + 0.0, + 9.0 + ] + } + }, + { + "semantic_class": "lamppost", + "obj_dim_mean": { + "Rows": 3, + "Cols": 1, + "Data": [ + 2.9999999999999999e-01, + 2.9999999999999999e-01, + 4.0 + ] + }, + "dim_covariance": { + "Rows": 3, + "Cols": 3, + "Data": [ + 2.2499999999999999e-02, + 0.0, + 0.0, + 0.0, + 2.2499999999999999e-02, + 0.0, + 0.0, + 0.0, + 9.0 + ] + } + }, + { + "semantic_class": "trashcan", + "obj_dim_mean": { + "Rows": 3, + "Cols": 1, + "Data": [ + 1.0, + 1.0, + 1.5000000000000000e+00 + ] + }, + "dim_covariance": { + "Rows": 3, + "Cols": 3, + "Data": [ + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 2.2500000000000000e+00 + ] + } + } + ] + }, + "camera_info": { + "camera_topic_to_camera_id": [ + { + "camera_topic": "/zed/zed_node/right/image_rect_color", + "camera_id": "2" + }, + { + "camera_topic": "/zed/zed_node/right/image_rect_color/compressed", + "camera_id": "2" + }, + { + "camera_topic": "/zed2i/zed_node/right/image_rect_color", + "camera_id": "2" + }, + { + "camera_topic": "/zed2i/zed_node/right/image_rect_color/compressed", + "camera_id": "2" + }, + { + "camera_topic": "/zed/zed_node/left/image_rect_color", + "camera_id": "1" + }, + { + "camera_topic": "/zed/zed_node/left/image_rect_color/compressed", + "camera_id": "1" + }, + { + "camera_topic": "/zed2i/zed_node/left/image_rect_color", + "camera_id": "1" + }, + { + "camera_topic": "/zed2i/zed_node/left/image_rect_color/compressed", + "camera_id": "1" + } + ] + }, + "bounding_box_front_end_params": { + "geometric_similarity_scorer_params": { + "max_merge_distance": 4.0, + "x_y_only_merge": 1 + }, + "feature_based_bb_association_params": { + "min_observations_for_local_est": 3, + "min_observations": 10, + "discard_candidate_after_num_frames": "40", + "min_bb_confidence": 2.0000000000000001e-01, + "required_min_conf_for_initialization": 0.0, + "min_overlapping_features_for_match": 3.0, + "feature_validity_window": "20", + "pending_obj_estimator_params": { + "object_residual_params": { + "object_observation_huber_loss_param": 5.0000000000000000e-01, + "shape_dim_prior_factor_huber_loss_param": 10.0, + "invalid_ellipsoid_error_val": 1000.0 + }, + "solver_params": { + "max_num_iterations": 500, + "allow_non_monotonic_steps": 1, + "function_tolerance": 9.9999999999999995e-07, + "gradient_tolerance": 1.0000000000000000e-10, + "parameter_tolerance": 1.0000000000000000e-08, + "initial_trust_region_radius": 10000.0, + "max_trust_region_radius": 1.0000000000000000e+16 + } + }, + "bounding_box_inflation_size": 10.0 + }, + "post_session_object_merge_params": { + "max_merge_distance": 2, + "x_y_only_merge": 1 + } + }, + "bounding_box_covariance_generator_params": { + "bounding_box_cov": { + "Rows": 4, + "Cols": 4, + "Data": [ + 900.0, + 0.0, + 0.0, + 0.0, + 0.0, + 900.0, + 0.0, + 0.0, + 0.0, + 0.0, + 900.0, + 0.0, + 0.0, + 0.0, + 0.0, + 900.0 + ] + }, + "near_edge_threshold": 25.0, + "image_boundary_variance": 40000.0 + }, + "sliding_window_params": { + "global_ba_frequency": "30", + "local_ba_window_size": "50" + }, + "optimization_factors_enabled_params": { + "min_low_level_feature_observations_per_frame": 50, + "include_object_factors": 1, + "include_visual_factors": 1, + "fix_poses": 0, + "fix_objects": 0, + "fix_visual_features": 0, + "fix_ltm_objects": 0, + "use_pom": 0, + "poses_prior_to_window_to_keep_constant": 5, + "min_object_observations": 10, + "min_low_level_feature_observations": 5, + "use_pose_graph_on_global_ba": 1, + "use_visual_features_on_global_ba": 0, + "use_pose_graph_on_final_global_ba": 1, + "use_visual_features_on_final_global_ba": 1 + }, + "object_visual_pose_graph_residual_params": { + "object_residual_params": { + "object_observation_huber_loss_param": 5.0000000000000000e-01, + "shape_dim_prior_factor_huber_loss_param": 10.0, + "invalid_ellipsoid_error_val": 1000.0 + }, + "visual_residual_params": { + "reprojection_error_huber_loss_param": 1.0 + }, + "long_term_map_params": { + "pair_huber_loss_param": 1.0 + }, + "relative_pose_factor_huber_loss": 1.0, + "relative_pose_cov_params": { + "transl_error_mult_for_transl_error": 2.5000000000000001e-02, + "transl_error_mult_for_rot_error": 2.5000000000000001e-02, + "rot_error_mult_for_transl_error": 2.5000000000000001e-02, + "rot_error_mult_for_rot_error": 2.5000000000000001e-02 + } + }, + "limit_traj_eval_params": { + "should_limit_trajectory_evaluation": 0, + "max_frame_id": 1 + }, + "sparsifier_params": { + "max_pose_inc_threshold_transl": 2.0000000000000001e-01, + "max_pose_inc_threshold_rot": 1.0000000000000001e-01 + } + } +} \ No newline at end of file diff --git a/include/file_io/cv_file_storage/config_file_storage_io.h b/include/file_io/cv_file_storage/config_file_storage_io.h index aca0ffdb..cdfa8fc2 100644 --- a/include/file_io/cv_file_storage/config_file_storage_io.h +++ b/include/file_io/cv_file_storage/config_file_storage_io.h @@ -1280,22 +1280,29 @@ class SerializableVisualFeatureParams int enforce_min_pixel_parallax_requirement = data_.enforce_min_pixel_parallax_requirement_ ? 1 : 0; - fs << kEnforceMinPixelParallaxRequirement + fs << kEnforceMinPixelParallaxRequirementLabel << enforce_min_pixel_parallax_requirement; int enforce_min_robot_pose_parallax_requirement = data_.enforce_min_robot_pose_parallax_requirement_ ? 1 : 0; - fs << kEnforceMinRobotPoseParallaxRequirement + fs << kEnforceMinRobotPoseParallaxRequirementLabel << enforce_min_robot_pose_parallax_requirement; - fs << kInlierEpipolarErrThresh << (double)data_.inlier_epipolar_err_thresh_; - fs << kCheckPastNFramesForEpipolarErr + fs << kInlierEpipolarErrThreshLabel + << (double)data_.inlier_epipolar_err_thresh_; + fs << kCheckPastNFramesForEpipolarErrLabel << (int)data_.check_past_n_frames_for_epipolar_err_; int enforce_epipolar_error_requirement = data_.enforce_epipolar_error_requirement_ ? 1 : 0; - fs << kEnforceEpipolarErrorRequirement + fs << kEnforceEpipolarErrorRequirementLabel << (int)enforce_epipolar_error_requirement; + + int early_votes_return_int = data_.early_votes_return_ ? 1 : 0; + fs << kEarlyVotesReturnLabel << (int)early_votes_return_int; + + fs << kVisualFeatureInlierMajorityPercentageLabel + << data_.visual_feature_inlier_majority_percentage_; fs << "}"; } @@ -1309,25 +1316,32 @@ class SerializableVisualFeatureParams data_.min_visual_feature_parallax_robot_orient_requirement_; int enforce_min_pixel_parallax_requirement = - node[kEnforceMinPixelParallaxRequirement]; + node[kEnforceMinPixelParallaxRequirementLabel]; data_.enforce_min_pixel_parallax_requirement_ = enforce_min_pixel_parallax_requirement != 0; int enforce_min_robot_pose_parallax_requirement = - node[kEnforceMinRobotPoseParallaxRequirement]; + node[kEnforceMinRobotPoseParallaxRequirementLabel]; data_.enforce_min_robot_pose_parallax_requirement_ = enforce_min_robot_pose_parallax_requirement != 0; - double inlier_epipolar_err_thresh = (double)node[kInlierEpipolarErrThresh]; + double inlier_epipolar_err_thresh = + (double)node[kInlierEpipolarErrThreshLabel]; data_.inlier_epipolar_err_thresh_ = inlier_epipolar_err_thresh; int check_past_n_frames_for_epipolar_err = - (int)node[kCheckPastNFramesForEpipolarErr]; + (int)node[kCheckPastNFramesForEpipolarErrLabel]; data_.check_past_n_frames_for_epipolar_err_ = check_past_n_frames_for_epipolar_err; int enforce_epipolar_error_requirement = - node[kEnforceEpipolarErrorRequirement]; + node[kEnforceEpipolarErrorRequirementLabel]; data_.enforce_epipolar_error_requirement_ = enforce_epipolar_error_requirement != 0; + + int early_votes_return_int = node[kEarlyVotesReturnLabel]; + data_.early_votes_return_ = early_votes_return_int != 0; + + data_.visual_feature_inlier_majority_percentage_ = + node[kVisualFeatureInlierMajorityPercentageLabel]; } protected: @@ -1345,16 +1359,19 @@ class SerializableVisualFeatureParams inline static const std::string kMinVisualFeatureParallaxRobotOrientRequirementLabel = "min_visual_feature_parallax_robot_orient_requirement"; - inline static const std::string kEnforceMinPixelParallaxRequirement = + inline static const std::string kEnforceMinPixelParallaxRequirementLabel = "enforce_min_pixel_parallax_requirement"; - inline static const std::string kEnforceMinRobotPoseParallaxRequirement = + inline static const std::string kEnforceMinRobotPoseParallaxRequirementLabel = "enforce_min_robot_pose_parallax_requirement"; - inline static const std::string kInlierEpipolarErrThresh = + inline static const std::string kInlierEpipolarErrThreshLabel = "inlier_epipolar_err_thresh"; - inline static const std::string kCheckPastNFramesForEpipolarErr = + inline static const std::string kCheckPastNFramesForEpipolarErrLabel = "check_past_n_frames_for_epipolar_err"; - inline static const std::string kEnforceEpipolarErrorRequirement = + inline static const std::string kEnforceEpipolarErrorRequirementLabel = "enforce_epipolar_error_requirement_"; + inline static const std::string kEarlyVotesReturnLabel = "early_votes_return"; + inline static const std::string kVisualFeatureInlierMajorityPercentageLabel = + "visual_feature_inlier_majority_percentage"; }; static void write(cv::FileStorage &fs, diff --git a/include/refactoring/configuration/full_ov_slam_config.h b/include/refactoring/configuration/full_ov_slam_config.h index 81df134a..0b8f3999 100644 --- a/include/refactoring/configuration/full_ov_slam_config.h +++ b/include/refactoring/configuration/full_ov_slam_config.h @@ -21,7 +21,7 @@ namespace vslam_types_refactor { // NOTE: This should be incremented every time the format of the configuration // data changes -const static int kCurrentConfigSchemaVersion = 12; +const static int kCurrentConfigSchemaVersion = 13; struct VisualFeatureParams { double reprojection_error_std_dev_; @@ -35,6 +35,9 @@ struct VisualFeatureParams { size_t check_past_n_frames_for_epipolar_err_; bool enforce_epipolar_error_requirement_; + bool early_votes_return_ = true; + double visual_feature_inlier_majority_percentage_ = 0.5; + bool operator==(const VisualFeatureParams &rhs) const { return (reprojection_error_std_dev_ == rhs.reprojection_error_std_dev_) && (min_visual_feature_parallax_pixel_requirement_ == @@ -51,7 +54,10 @@ struct VisualFeatureParams { (check_past_n_frames_for_epipolar_err_ == rhs.check_past_n_frames_for_epipolar_err_) && (enforce_epipolar_error_requirement_ == - rhs.enforce_epipolar_error_requirement_); + rhs.enforce_epipolar_error_requirement_) && + (early_votes_return_ == rhs.early_votes_return_) && + (visual_feature_inlier_majority_percentage_ == + rhs.visual_feature_inlier_majority_percentage_); } bool operator!=(const VisualFeatureParams &rhs) const { diff --git a/include/refactoring/offline/offline_problem_runner.h b/include/refactoring/offline/offline_problem_runner.h index 6a960e1f..e63e2e6e 100644 --- a/include/refactoring/offline/offline_problem_runner.h +++ b/include/refactoring/offline/offline_problem_runner.h @@ -199,6 +199,20 @@ class OfflineProblemRunner { } } + // Run the final optimization (refinement) + if (!runOptimizationIteration(0, + max_frame_id, + problem_data, + optimization_factors_enabled_params, + optimization_scope_params, + max_frame_id, + opt_logger, + pose_graph, + problem, + 1)) { + return false; + } + visualization_callback_(problem_data, pose_graph, 0, @@ -366,7 +380,7 @@ class OfflineProblemRunner { bool run_visual_feature_opt = true; if (global_ba) { bool run_pgo; - if (next_frame_id == max_frame_id) { + if ((next_frame_id == max_frame_id) && (attempt_num > 0)) { run_pgo = optimization_factors_enabled_params .use_pose_graph_on_final_global_ba_; if (run_pgo) { @@ -811,7 +825,7 @@ class OfflineProblemRunner { // Until there are no more objects to merge, check if a merge should be // performed, and then if so, rerun the optimization to update the estimates - int post_process_round = 1; + int post_process_round = 2; while (object_merger_(pose_graph)) { ceres::Problem merged_problem; optimizer_.clearPastOptimizationData(); diff --git a/include/refactoring/visual_feature_frontend/visual_feature_front_end.h b/include/refactoring/visual_feature_frontend/visual_feature_front_end.h index b8a561c4..bbba49be 100644 --- a/include/refactoring/visual_feature_frontend/visual_feature_front_end.h +++ b/include/refactoring/visual_feature_frontend/visual_feature_front_end.h @@ -228,7 +228,9 @@ class VisualFeatureFrontend { const bool &enforce_min_robot_pose_parallax_requirement, const double &inlier_epipolar_err_thresh, const size_t &check_pase_n_frames_for_epipolar_err, - const bool &enforce_epipolar_error_requirement) + const bool &enforce_epipolar_error_requirement, + const bool &early_votes_return, + const double &inlier_majority_percentage) : gba_checker_(gba_checker), reprojection_error_provider_(reprojection_error_provider), min_visual_feature_parallax_pixel_requirement_( @@ -245,7 +247,9 @@ class VisualFeatureFrontend { check_pase_n_frames_for_epipolar_err_( check_pase_n_frames_for_epipolar_err), enforce_epipolar_error_requirement_( - enforce_epipolar_error_requirement) {} + enforce_epipolar_error_requirement), + early_votes_return_(early_votes_return), + inlier_majority_percentage_(inlier_majority_percentage) {} /** * @brief @@ -474,8 +478,9 @@ class VisualFeatureFrontend { double inlier_epipolar_err_thresh_ = 8.0; size_t check_pase_n_frames_for_epipolar_err_ = 5; bool enforce_epipolar_error_requirement_ = true; + bool early_votes_return_ = true; - const double inlier_majority_percentage_ = 0.5; + double inlier_majority_percentage_ = 0.5; private: void getFactorsByFeatureIdFromPoseGraph_( @@ -586,9 +591,11 @@ class VisualFeatureFrontend { n_voters++; } // TODO maybe add this to configuration as well - - return (((double)votes) / n_voters) > inlier_majority_percentage_; + if (early_votes_return_) { + return (((double)votes) / n_voters) > inlier_majority_percentage_; + } } + return (((double)votes) / n_voters) > inlier_majority_percentage_; } bool isReprojectionErrorFactorInlierInPoseGraph_( diff --git a/include/run_optimization_utils/optimization_runner.h b/include/run_optimization_utils/optimization_runner.h index 67fe09fc..ecc43c64 100644 --- a/include/run_optimization_utils/optimization_runner.h +++ b/include/run_optimization_utils/optimization_runner.h @@ -313,7 +313,9 @@ bool runFullOptimization( .enforce_min_robot_pose_parallax_requirement_, config.visual_feature_params_.inlier_epipolar_err_thresh_, config.visual_feature_params_.check_past_n_frames_for_epipolar_err_, - config.visual_feature_params_.enforce_epipolar_error_requirement_); + config.visual_feature_params_.enforce_epipolar_error_requirement_, + config.visual_feature_params_.early_votes_return_, + config.visual_feature_params_.visual_feature_inlier_majority_percentage_); std::function