From 7ba7af903809caca19a5c6a34735d52b3415e521 Mon Sep 17 00:00:00 2001 From: viktor Date: Tue, 16 Apr 2024 13:34:01 +0200 Subject: [PATCH] Accelerated initialization for erroneous observations --- src/pose_calculator.cpp | 25 +++++++++++++++++-------- 1 file changed, 17 insertions(+), 8 deletions(-) diff --git a/src/pose_calculator.cpp b/src/pose_calculator.cpp index e12672e..8381452 100644 --- a/src/pose_calculator.cpp +++ b/src/pose_calculator.cpp @@ -76,7 +76,7 @@ #define MAX_HYPOTHESIS_AGE 1.5 #define MAX_INIT_ITERATIONS 10000 -#define MAX_MUTATION_REFINE_ITERATIONS 10000 +#define MAX_MUTATION_REFINE_ITERATIONS 1000 #define SIMILAR_ERRORS_THRESHOLD sqr(1) @@ -2432,13 +2432,22 @@ namespace uvdar { auto [hypotheses_init, errors_init] = getViableInitialHyptheses(points, furthest_position, target, image_index, fromcam_tf, tocam_tf, INITIAL_ROUGH_HYPOTHESIS_COUNT, time); profiler.addValue("InitialHypotheses"); - - auto hypotheses_refined = refineByMutation(model_, points, hypotheses_init, tocam_tf, image_index, target, ERROR_THRESHOLD_MUTATION_1(image_index), 1.0, 1.0, INITIAL_HYPOTHESIS_COUNT); - profiler.addValue("Initial Mutation 1"); - hypotheses_refined = refineByMutation(model_, points, hypotheses_refined, tocam_tf, image_index, target, ERROR_THRESHOLD_MUTATION_2(image_index), 1.0, 1.0, INITIAL_HYPOTHESIS_COUNT); - profiler.addValue("Initial Mutation 2"); - hypotheses_refined = refineByMutation(model_, points, hypotheses_refined, tocam_tf, image_index, target, ERROR_THRESHOLD_MUTATION_3(image_index), 1.0, 1.0, INITIAL_HYPOTHESIS_COUNT); - profiler.addValue("Initial Mutation 3"); + double ratio_found = (double)(hypotheses_init.size()) / (double)(INITIAL_ROUGH_HYPOTHESIS_COUNT); + ROS_INFO(" Ratio Init: %f", 100*ratio_found ); + + int desired_count = (int)(ratio_found*INITIAL_HYPOTHESIS_COUNT); + auto hypotheses_refined = refineByMutation(model_, points, hypotheses_init, tocam_tf, image_index, target, ERROR_THRESHOLD_MUTATION_1(image_index), 1.0, 1.0, desired_count); + /* profiler.addValue("Initial Mutation 1"); */ + ratio_found = (double)(hypotheses_refined.size()) / desired_count; + ROS_INFO(" Ratio Refin 1: %f", 100*ratio_found ); + desired_count = (int)(ratio_found*desired_count); + hypotheses_refined = refineByMutation(model_, points, hypotheses_refined, tocam_tf, image_index, target, ERROR_THRESHOLD_MUTATION_2(image_index), 1.0, 1.0, desired_count); + /* profiler.addValue("Initial Mutation 2"); */ + ratio_found = (double)(hypotheses_refined.size()) / desired_count; + ROS_INFO(" Ratio Refin 2: %f", 100*ratio_found ); + desired_count = (int)(ratio_found*desired_count); + hypotheses_refined = refineByMutation(model_, points, hypotheses_refined, tocam_tf, image_index, target, ERROR_THRESHOLD_MUTATION_3(image_index), 1.0, 1.0, desired_count); + /* profiler.addValue("Initial Mutation 3"); */ int static_hypothesis_count = (int)(hypotheses_refined.size()); for (int i=0; i