Merge pull request #20 from rsoussan/teblid_tuning_updateS

Bug fixes and tuning updates

astrobee/config/localization.config

@@ -29,11 +29,11 @@ brisk_num_ransac_iterations = 100
 brisk_early_break_landmarks = 100
 brisk_histogram_equalization = 1
 brisk_check_essential_matrix = false
-brisk_essential_ransac_iterations = 2000
+brisk_essential_ransac_iterations = 0
 brisk_add_similar_images = false
 brisk_add_best_previous_image = false
 brisk_hamming_distance = 90
-brisk_goodness_ratio = 0.8
+brisk_goodness_ratio = 10000
 brisk_use_clahe = false 
 brisk_num_extra_localization_db_images = 0
 -- Detection Params
@@ -42,14 +42,16 @@ brisk_default_threshold = 90.0
 brisk_max_threshold = 110.0
 brisk_min_features = 200 
 brisk_max_features = 800
+brisk_too_many_ratio = 1.25 
+brisk_too_few_ratio = 0.8
 brisk_detection_retries = 1
 -- Localizer Threshold Params
 brisk_success_history_size = 10
 brisk_min_success_rate = 0
 brisk_max_success_rate = 1
-brisk_adjust_hamming_distance = false
-brisk_min_hamming_distance = 90
-brisk_max_hamming_distance = 90
+brisk_adjust_num_similar = false
+brisk_min_num_similar = 20
+brisk_max_num_similar = 20
 
 -- TEBLID512
 teblid512_num_similar = 20
@@ -69,19 +71,21 @@ teblid512_num_extra_localization_db_images = 0
 
 -- Detection Params
 teblid512_min_threshold = 20.0
-teblid512_default_threshold = 60.0
+teblid512_default_threshold = 72.0
 teblid512_max_threshold = 110.0
 teblid512_min_features = 1000
 teblid512_max_features = 3000
+teblid512_too_many_ratio = 1.05 
+teblid512_too_few_ratio = 0.95
 teblid512_detection_retries = 1
 
 -- Localizer Threshold Params
 teblid512_success_history_size = 10
 teblid512_min_success_rate = 0.3
 teblid512_max_success_rate = 0.9
-teblid512_adjust_hamming_distance = false
-teblid512_min_hamming_distance = 85
-teblid512_max_hamming_distance = 85
+teblid512_adjust_num_similar = true 
+teblid512_min_num_similar = 15
+teblid512_max_num_similar = 20
 
 -- TEBLID256
 teblid256_num_similar = 20
@@ -101,16 +105,18 @@ teblid256_num_extra_localization_db_images = 0
 
 -- Detection Params
 teblid256_min_threshold = 20.0
-teblid256_default_threshold = 60.0
+teblid256_default_threshold = 72.0
 teblid256_max_threshold = 110.0
 teblid256_min_features = 1000
 teblid256_max_features = 3000
+teblid256_too_many_ratio = 1.05 
+teblid256_too_few_ratio = 0.95
 teblid256_detection_retries = 1
 
 -- Localizer Threshold Params
 teblid256_success_history_size = 10
 teblid256_min_success_rate = 0.3
 teblid256_max_success_rate = 0.9
-teblid256_adjust_hamming_distance = false
-teblid256_min_hamming_distance = 85
-teblid256_max_hamming_distance = 85
+teblid256_adjust_num_similar = true 
+teblid256_min_num_similar = 15
+teblid256_max_num_similar = 20

astrobee/config/localization/graph_localizer.config

@@ -61,7 +61,7 @@ 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_ideal_duration = 15 
 gl_na_pose_min_num_states = 3 
 gl_na_pose_max_num_states = 5
 gl_na_pose_model_huber_k = world_huber_k

astrobee/config/localization/localization_manager.config

@@ -108,7 +108,7 @@ pipelines = {
  { id = "ml", name = "Sparse map", ekf_input = 0,
  needs_filter = true, optical_flow = true, timeout = 1.0, max_confidence = 0,
  enable_topic = "loc/ml/enable", enable_timeout = 20.0,
- reg_topic = "loc/ml/registration", reg_timeout = 3.0,
+ reg_topic = "loc/ml/registration", reg_timeout = 30.0,
  feat_topic = "loc/ml/features", feat_timeout = 600.0, feat_threshold = 3 },
  -- AR Tags
  { id = "ar", name = "AR Tags", ekf_input = 1,

communications/ff_msgs/msg/VisualLandmarks.msg

@@ -22,3 +22,4 @@ Header header # header with timestamp
 uint32 camera_id # image ID, associated with registration pulse
 geometry_msgs/Pose pose # estimated camera pose from features
 ff_msgs/VisualLandmark[] landmarks # list of all landmarks
+float32 runtime # Time it took to calculate the pose and matching landmarks

localization/interest_point/include/interest_point/matching.h

@@ -31,7 +31,8 @@ namespace interest_point {
  class DynamicDetector {
  public:
  DynamicDetector(int min_features, int max_features, int retries,
- double min_thresh, double default_thresh, double max_thresh);
+ double min_thresh, double default_thresh, double max_thresh,
+ double too_many_ratio, double too_few_ratio);
  virtual ~DynamicDetector(void) {}
  void Detect(const cv::Mat& image,
  std::vector<cv::KeyPoint>* keypoints,
@@ -49,8 +50,8 @@ namespace interest_point {
  int last_keypoint_count(void) { return last_keypoint_count_; }
 
  protected:
- unsigned int min_features_, max_features_, max_retries_;
- double min_thresh_, default_thresh_, max_thresh_, dynamic_thresh_;
+ int min_features_, max_features_, max_retries_;
+ double min_thresh_, default_thresh_, max_thresh_, dynamic_thresh_, too_few_ratio_, too_many_ratio_;
  int last_keypoint_count_;
  };
 
@@ -65,14 +66,14 @@ namespace interest_point {
 
  public:
  // Here on purpose invalid values are set, so the user explicitly sets them.
- FeatureDetector(std::string const& detector_name = "SURF",
- int min_features = 0, int max_features = 0, int retries = 0,
- double min_thresh = 0, double default_thresh = 0, double max_thresh = 0);
+ FeatureDetector(std::string const& detector_name = "SURF", int min_features = 0, int max_features = 0,
+ int retries = 0, double min_thresh = 0, double default_thresh = 0, double max_thresh = 0,
+ double too_many_ratio = 0, double too_few_ratio = 0);
  ~FeatureDetector(void);
 
- void Reset(std::string const& detector_name,
-  int min_features = 0, int max_features = 0, int retries = 0,
-  double min_thresh = 0, double default_thresh = 0, double max_thresh = 0);
+ void Reset(std::string const& detector_name, int min_features = 0, int max_features = 0, int retries = 0,
+ double min_thresh = 0, double default_thresh = 0, double max_thresh = 0, double too_many_ratio = 0,
+ double too_few_ratio = 0);
 
  void Detect(const cv::Mat& image, std::vector<cv::KeyPoint>* keypoints,
  cv::Mat* keypoints_description);