added protection against switching to an invalid map

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

@@ -63,18 +63,27 @@ bool LocalizationNodelet::ResetMap(const std::string& map_file) {
   map_.reset();
   map_ = std::make_shared<sparse_mapping::SparseMap>(map_file, true);
   inst_.reset(new Localizer(map_.get()));
-  // Sanity check that map contains a valid detector type
-  const auto detector_name = map_->GetDetectorName();
-  if (detector_name != "ORGBRISK"  || detector_name != "TEBLID512" || detector_name != "TEBLID256") {
+
+  // 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 feature detector type: " << detector_name);
-    map_.reset();
-    inst_.reset();
-    return false;
+                                                     << " 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()));
+    } 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;
   }
 
-  // Check to see if any params were changed when map was reset
-  ReadParams();
   enabled_ = true;
   return true;
 }
@@ -97,6 +106,7 @@ void LocalizationNodelet::Initialize(ros::NodeHandle* nh) {
   it_.reset(new image_transport::ImageTransport(*nh));
   map_.reset();
   map_ = std::make_shared<sparse_mapping::SparseMap>(map_file, true);
+  last_valid_map_file_ = map_file;
 
 
   inst_.reset(new Localizer(map_.get()));
@@ -133,20 +143,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_) {
+    return inst_->ReadParams(config_, fatal_failure);
   }
-  if (inst_) inst_->ReadParams(config_);
+  return true;
 }
 
 bool LocalizationNodelet::EnableService(ff_msgs::SetBool::Request & req, ff_msgs::SetBool::Response & res) {

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/src/sparse_mapping.cc

@@ -115,15 +115,21 @@ namespace sparse_mapping {
 // Logic for implementing if two histogram equalization flags are compatible.
 // This flag can be either 0 (false), 1 (true), or 2 (unknown). Be tolerant
 // of unknown values, but intolerant when true and false are mixed.
-void sparse_mapping::HistogramEqualizationCheck(int histogram_equalization1,
-                                                int histogram_equalization2) {
+bool sparse_mapping::HistogramEqualizationCheck(int histogram_equalization1,
+                                                int histogram_equalization2, bool fatal_failure) {
   // Ignore if either has unknown equalization value
   if (histogram_equalization1 == HistogramEqualizationType::kUnknown ||
       histogram_equalization2 == HistogramEqualizationType::kUnknown) {
-    return;
+    return true;
   } else if (histogram_equalization1 != histogram_equalization2) {
-    LOG(FATAL) << "Incompatible values of histogram equalization detected.";
+    if (fatal_failure) {
+      LOG(FATAL) << "Incompatible values of histogram equalization detected.";
+    } else {
+      LOG(ERROR) << "Incompatible values of histogram equalization detected.";
+    }
+    return false;
   }
+  return true;
 }
 
 bool sparse_mapping::IsBinaryDescriptor(std::string const& descriptor) {