Fixes from ISAAC16

astrobee/config/localization.config

@@ -54,69 +54,69 @@ brisk_min_num_similar = 20
 brisk_max_num_similar = 20
 
 -- TEBLID512
-teblid512_num_similar = 20
+teblid512_num_similar = 18
 teblid512_min_query_score_ratio = 0.45
 teblid512_ransac_inlier_tolerance = 3
 teblid512_num_ransac_iterations = 1000
 teblid512_early_break_landmarks = 100
 teblid512_histogram_equalization = 3
-teblid512_check_essential_matrix = true 
+teblid512_check_essential_matrix = false 
 teblid512_essential_ransac_iterations = 2000
 teblid512_add_similar_images = true 
 teblid512_add_best_previous_image = true
-teblid512_hamming_distance = 85
+teblid512_hamming_distance = 90 
 teblid512_goodness_ratio = 0.8
 teblid512_use_clahe = true 
 teblid512_num_extra_localization_db_images = 0
 
 -- Detection Params
-teblid512_min_threshold = 20.0
+teblid512_min_threshold = 30.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_max_features = 2750
+teblid512_too_many_ratio = 1.07 
 teblid512_too_few_ratio = 0.95
 teblid512_detection_retries = 1
 
 -- Localizer Threshold Params
 teblid512_success_history_size = 10
-teblid512_min_success_rate = 0.5
+teblid512_min_success_rate = 0.4
 teblid512_max_success_rate = 0.9
 teblid512_adjust_num_similar = true 
 teblid512_min_num_similar = 15
-teblid512_max_num_similar = 20
+teblid512_max_num_similar = 18
 
 -- TEBLID256
-teblid256_num_similar = 20
+teblid256_num_similar = 18
 teblid256_min_query_score_ratio = 0.45
 teblid256_ransac_inlier_tolerance = 3
 teblid256_num_ransac_iterations = 1000
 teblid256_early_break_landmarks = 100
 teblid256_histogram_equalization = 3
-teblid256_check_essential_matrix = true 
+teblid256_check_essential_matrix = false 
 teblid256_essential_ransac_iterations = 2000
 teblid256_add_similar_images = true 
 teblid256_add_best_previous_image = true
-teblid256_hamming_distance = 85
+teblid256_hamming_distance = 90 
 teblid256_goodness_ratio = 0.8
 teblid256_use_clahe = true 
 teblid256_num_extra_localization_db_images = 0
 
 -- Detection Params
-teblid256_min_threshold = 20.0
+teblid256_min_threshold = 30.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_max_features = 2750
+teblid256_too_many_ratio = 1.07 
 teblid256_too_few_ratio = 0.95
 teblid256_detection_retries = 1
 
 -- Localizer Threshold Params
 teblid256_success_history_size = 10
-teblid256_min_success_rate = 0.5
+teblid256_min_success_rate = 0.4
 teblid256_max_success_rate = 0.9
 teblid256_adjust_num_similar = true 
 teblid256_min_num_similar = 15
-teblid256_max_num_similar = 20
+teblid256_max_num_similar = 18

localization/localization_node/include/localization_node/localization.h

@@ -45,7 +45,7 @@ struct ThresholdParams {
 class Localizer {
  public:
   explicit Localizer(sparse_mapping::SparseMap* map);
-  void ReadParams(config_reader::ConfigReader& config);
+  bool ReadParams(config_reader::ConfigReader& config, bool fatal_failure = true);
   bool Localize(cv_bridge::CvImageConstPtr image_ptr, ff_msgs::VisualLandmarks* vl,
      Eigen::Matrix2Xd* image_keypoints = NULL);
  private:

localization/localization_node/include/localization_node/localization_nodelet.h

@@ -44,7 +44,10 @@ class LocalizationNodelet : public ff_util::FreeFlyerNodelet {
   virtual void Initialize(ros::NodeHandle* nh);
 
  private:
-  void ReadParams(void);
+  // Wrapper function that calls ReadParams but does not take a param,
+  // required for configuring with a config timer.
+  void ReadParamsWrapper();
+  bool ReadParams(bool fatal_failure = true);
   bool ResetMap(const std::string& map_file);
   void Run(void);
   void Localize(void);
@@ -56,6 +59,7 @@ class LocalizationNodelet : public ff_util::FreeFlyerNodelet {
   std::shared_ptr<sparse_mapping::SparseMap> map_;
   std::shared_ptr<std::thread> thread_;
   config_reader::ConfigReader config_;
+  std::string last_valid_map_file_;
   ros::Timer config_timer_;
 
   std::shared_ptr<image_transport::ImageTransport> it_;

localization/localization_node/src/localization.cc

@@ -29,7 +29,7 @@ namespace localization_node {
 
 Localizer::Localizer(sparse_mapping::SparseMap* map): map_(map) {}
 
-void Localizer::ReadParams(config_reader::ConfigReader& config) {
+bool Localizer::ReadParams(config_reader::ConfigReader& config, bool fatal_failure) {
   camera::CameraParameters cam_params(&config, "nav_cam");
   std::string prefix;
   const auto detector_name = map_->GetDetectorName();
@@ -40,7 +40,12 @@ void Localizer::ReadParams(config_reader::ConfigReader& config) {
   } else if (detector_name == "TEBLID256") {
     prefix = "teblid256_";
   } else {
-    ROS_FATAL_STREAM("Invalid detector: " << detector_name);
+    if (fatal_failure) {
+      ROS_FATAL_STREAM("Invalid detector: " << detector_name);
+    } else {
+      ROS_ERROR_STREAM("Invalid detector: " << detector_name);
+    }
+    return false;
   }
 
   // Loc params
@@ -86,17 +91,24 @@ void Localizer::ReadParams(config_reader::ConfigReader& config) {
 
   // This check must happen before the histogram_equalization flag is set into the map
   // to compare with what is there already.
-  sparse_mapping::HistogramEqualizationCheck(map_->GetHistogramEqualization(),
-                                             loc_params.histogram_equalization);
+  if (!sparse_mapping::HistogramEqualizationCheck(map_->GetHistogramEqualization(),
+                                             loc_params.histogram_equalization, fatal_failure)) return false;
+
   // Check consistency between clahe params
   if (loc_params.use_clahe && (loc_params.histogram_equalization != 3 || map_->GetHistogramEqualization() != 3)) {
-    ROS_FATAL("Invalid clahe and histogram equalization settings.");
+    if (fatal_failure) {
+      ROS_FATAL("Invalid clahe and histogram equalization settings.");
+    } else {
+      ROS_ERROR("Invalid clahe and histogram equalization settings.");
+    }
+    return false;
   }
 
   map_->SetCameraParameters(cam_params);
   map_->SetLocParams(loc_params);
   map_->SetDetectorParams(min_features, max_features, detection_retries,
                           min_threshold, default_threshold, max_threshold, too_many_ratio, too_few_ratio);
+  return true;
 }
 
 bool Localizer::Localize(cv_bridge::CvImageConstPtr image_ptr, ff_msgs::VisualLandmarks* vl,

localization/localization_node/src/localization_nodelet.cc

@@ -59,10 +59,33 @@ bool LocalizationNodelet::ResetMap(const std::string& map_file) {
   while (processing_image_) {
     usleep(100000);
   }
-  map_.reset(new sparse_mapping::SparseMap(map_file, true));
+  // Reset map before creating new one to avoid storing two maps in memory at the same time
+  map_.reset();
+  map_ = std::make_shared<sparse_mapping::SparseMap>(map_file, true);
   inst_.reset(new Localizer(map_.get()));
-  // Check to see if any params were changed when map was reset
-  ReadParams();
+
+  // Check to see if any params were changed when map was reset.
+  // Make sure they are valid.
+  if (!ReadParams(false)) {
+    ROS_ERROR_STREAM("Failed to switch to map file " << map_file
+                                                     << " due to invalid params.");
+    if (!last_valid_map_file_.empty()) {
+      ROS_ERROR_STREAM("Reverting to last map file " << last_valid_map_file_);
+      map_.reset();
+      map_ = std::make_shared<sparse_mapping::SparseMap>(last_valid_map_file_, true);
+      inst_.reset(new Localizer(map_.get()));
+      ReadParams();
+      enabled_ = true;
+    } else {
+      ROS_ERROR_STREAM("No last valid map, please reset with a valid map. Not localizing.");
+      map_.reset();
+      inst_.reset();
+    }
+    return false;
+  } else {
+    last_valid_map_file_ = map_file;
+  }
+
   enabled_ = true;
   return true;
 }
@@ -83,7 +106,11 @@ void LocalizationNodelet::Initialize(ros::NodeHandle* nh) {
 
   // Reset all internal shared pointers
   it_.reset(new image_transport::ImageTransport(*nh));
-  map_.reset(new sparse_mapping::SparseMap(map_file, true));
+  map_.reset();
+  map_ = std::make_shared<sparse_mapping::SparseMap>(map_file, true);
+  last_valid_map_file_ = map_file;
+
+
   inst_.reset(new Localizer(map_.get()));
 
   registration_publisher_ = nh->advertise<ff_msgs::CameraRegistration>(
@@ -118,20 +145,28 @@ void LocalizationNodelet::Initialize(ros::NodeHandle* nh) {
   cv::setNumThreads(num_threads);
 
   config_timer_ = nh->createTimer(ros::Duration(1), [this](ros::TimerEvent e) {
-      config_.CheckFilesUpdated(std::bind(&LocalizationNodelet::ReadParams, this));}, false, true);
+      config_.CheckFilesUpdated(std::bind(&LocalizationNodelet::ReadParamsWrapper, this));}, false, true);
 
   enable_srv_ = nh->advertiseService(SERVICE_LOCALIZATION_ML_ENABLE, &LocalizationNodelet::EnableService, this);
   reset_map_srv_ = nh->advertiseService(SERVICE_LOCALIZATION_RESET_MAP, &LocalizationNodelet::ResetMapService, this);
   reset_map_loc_client_ = nh->serviceClient<ff_msgs::ResetMap>(
                                                 SERVICE_LOCALIZATION_RESET_MAP_LOC);
 }
 
-void LocalizationNodelet::ReadParams(void) {
+
+void LocalizationNodelet::ReadParamsWrapper() {
+  ReadParams(true);
+}
+
+bool LocalizationNodelet::ReadParams(bool fatal_failure) {
   if (!config_.ReadFiles()) {
     ROS_ERROR("Failed to read config files.");
-    return;
+    return false;
   }
-  if (inst_) inst_->ReadParams(config_);
+  if (inst_) {
+    return inst_->ReadParams(config_, fatal_failure);
+  }
+  return true;
 }
 
 bool LocalizationNodelet::EnableService(ff_msgs::SetBool::Request & req, ff_msgs::SetBool::Response & res) {
@@ -189,6 +224,7 @@ void LocalizationNodelet::ImageCallback(const sensor_msgs::ImageConstPtr& msg) {
 }
 
 void LocalizationNodelet::Localize(void) {
+  if (!inst_) return;
   ff_msgs::VisualLandmarks vl;
   Eigen::Matrix2Xd image_keypoints;
 

localization/localization_node/tools/set_start_pose.py

@@ -0,0 +1,95 @@
+#!/usr/bin/python3
+#
+# 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.
+"""
+Initializes the localizer with a defined pose by sending a ff_msgs VisualLandmarks message 
+with the pose value. Note the pose is expected in the robot nav camera frame.
+"""
+
+import argparse
+import os
+import sys
+
+import rospy
+import std_msgs.msg
+from ff_msgs.msg import VisualLandmark, VisualLandmarks
+
+
+def publish_pose(pose):
+    # Init publisher
+    pub = rospy.Publisher("/loc/ml/features", VisualLandmarks, queue_size=1)
+    rospy.init_node("PosePublisher")
+    # Sleep so node can subscribe to /clock topic and produce a valid header time
+    rospy.sleep(5)
+
+    msg = VisualLandmarks()
+    msg.header = std_msgs.msg.Header()
+    msg.header.stamp = rospy.Time.now()
+    # Subtract 2 seconds from stamp to simulate localization time,
+    # allows VIO messages to accrue that span before and after the vl message
+    msg.header.stamp.secs = msg.header.stamp.secs - 2
+    msg.header.frame_id = "world"
+    msg.camera_id = 0
+    # Set pose values
+    msg.pose.position.x = pose[0]
+    msg.pose.position.y = pose[1]
+    msg.pose.position.z = pose[2]
+    msg.pose.orientation.x = pose[3]
+    msg.pose.orientation.y = pose[4]
+    msg.pose.orientation.z = pose[5]
+    msg.pose.orientation.w = pose[6]
+    # Set empty landmark features, make sure to have > 5 so msg is considered valid
+    for i in range(0, 5):
+        landmark = VisualLandmark()
+        landmark.x = 0
+        landmark.y = 0
+        landmark.z = 0
+        landmark.u = 0
+        landmark.v = 0
+        msg.landmarks.append(landmark)
+
+    pub.publish(msg)
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(
+        description=__doc__, formatter_class=argparse.ArgumentDefaultsHelpFormatter
+    )
+    parser.add_argument(
+        "pose",
+        nargs="+",
+        type=float,
+        help="Start pose in the format: x y z qx qy qz qw. If --position-only used, only x y z are required.",
+    )
+    parser.add_argument(
+        "-p",
+        "--position-only",
+        dest="initialize_position_only",
+        action="store_true",
+        help="Initialize a pose using only the position, the orientation will be set to identity.",
+    )
+    args = parser.parse_args()
+    if not args.initialize_position_only and len(args.pose) != 7:
+        print("Pose requires 7 fields.")
+        sys.exit()
+    if args.initialize_position_only and len(args.pose) != 3:
+        print("Position only pose requires 3 fields.")
+        sys.exit()
+    if args.initialize_position_only:
+        args.pose.extend([0, 0, 0, 1])
+    publish_pose(args.pose)

localization/sparse_mapping/include/sparse_mapping/sparse_mapping.h

@@ -76,7 +76,7 @@ Eigen::Quaternion<double> slerp_n(std::vector<double> const& W, std::vector<Eige
 bool IsBinaryDescriptor(std::string const& descriptor);
 
 // Logic for implementing if two histogram equalization flags are compatible
-void HistogramEqualizationCheck(int histogram_equalization1, int histogram_equalization2);
+bool HistogramEqualizationCheck(int histogram_equalization1, int histogram_equalization2, bool fatal_failure = true);
 
 // Writes the NVM control network format.
 void WriteNVM(std::vector<Eigen::Matrix2Xd> const& cid_to_keypoint_map, std::vector<std::string> const& cid_to_filename,

localization/sparse_mapping/scripts/make_surf_map.py

@@ -1,4 +1,4 @@
-#!/usr/bin/python
+#!/usr/bin/python3
 #
 # Copyright (c) 2017, United States Government, as represented by the
 # Administrator of the National Aeronautics and Space Administration.

localization/sparse_mapping/scripts/make_surf_maps.py

@@ -1,4 +1,4 @@
-#!/usr/bin/python
+#!/usr/bin/python3
 #
 # Copyright (c) 2017, United States Government, as represented by the
 # Administrator of the National Aeronautics and Space Administration.

localization/sparse_mapping/scripts/make_teblid512_map.py

@@ -1,4 +1,4 @@
-#!/usr/bin/python
+#!/usr/bin/python3
 #
 # Copyright (c) 2017, United States Government, as represented by the
 # Administrator of the National Aeronautics and Space Administration.
@@ -53,7 +53,7 @@ def make_teblid512_map(surf_map, world, robot_name, map_directory=None):
     if map_directory:
         os.chdir(map_directory)
     build_teblid512_map_command = (
-        "rosrun sparse_mapping build_map -rebuild -histogram_equalization -use_clahe -output_map "
+        "rosrun sparse_mapping build_map -rebuild -histogram_equalization -use_clahe -min_brisk_features 3000 -output_map "
         + teblid512_map_full_path
     )
     lu.run_command_and_save_output(

localization/sparse_mapping/scripts/process_sequential_images.py

@@ -1,4 +1,4 @@
-#!/usr/bin/python
+#!/usr/bin/python3
 #
 # Copyright (c) 2017, United States Government, as represented by the
 # Administrator of the National Aeronautics and Space Administration.