Adding support for SURF Features

.gitignore

@@ -98,3 +98,8 @@ tags
 .ctags
 
 .vscode
+
+# submodules/
+tools/localization_analysis/scripts/
+localization/sparse_mapping/scripts/
+localization/sparse_mapping/tools/

localization/interest_point/src/matching.cc

@@ -31,6 +31,8 @@
 // same settings!
 // TODO(oalexan1): Ideally the settings used here must be saved in the
 // map file, for the localize executable to read them from there.
+DEFINE_bool(verbose, false,
+            "If true, print out information about the localization process.");
 DEFINE_int32(hamming_distance, 90,
              "A smaller value keeps fewer but more reliable binary descriptor matches.");
 DEFINE_double(goodness_ratio, 0.8,
@@ -50,7 +52,7 @@ DEFINE_int32(max_surf_features, 5000,
              "Maximum number of features to be computed using SURF.");
 DEFINE_double(min_surf_threshold, 1.1,
               "Minimum threshold for feature detection using SURF.");
-DEFINE_double(default_surf_threshold, 10,
+DEFINE_double(default_surf_threshold, 400,
               "Default threshold for feature detection using SURF.");
 DEFINE_double(max_surf_threshold, 1000,
               "Maximum threshold for feature detection using SURF.");
@@ -99,6 +101,16 @@ namespace interest_point {
       else
         break;
     }
+    if (FLAGS_verbose) {
+      if (keypoints->size() < min_features_)
+        LOG(WARNING) << "Max retries reached. Found " << keypoints->size()
+                    << " keypoints which is less than min of " << min_features_
+                    << " keypoints. Consider decreasing the default threshold.";
+      else if (keypoints->size() > max_features_)
+        LOG(WARNING) << "Max retries reached. Found " << keypoints->size()
+                    << " keypoints which exeeds max of " << max_features_
+                    << " keypoints. Consider increasing the default threshold.";
+    }
     ComputeImpl(image, keypoints, keypoints_description);
   }
 

localization/localization_node/src/localization.cc

@@ -100,7 +100,7 @@ bool Localizer::Localize(cv_bridge::CvImageConstPtr image_ptr, ff_msgs::VisualLa
   std::vector<Eigen::Vector2d> observations;
   if (!map_->Localize(image_descriptors, *image_keypoints,
                                &camera, &landmarks, &observations)) {
-    // LOG(INFO) << "Failed to localize image.";
+    LOG(INFO) << "Failed to localize image.";
     return false;
   }
 

localization/sparse_mapping/include/sparse_mapping/brisk_image_database.h

@@ -0,0 +1,28 @@
+/* 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.
+ */
+
+#ifndef SPARSE_MAPPING_BRISK_IMAGE_DATABASE_H_
+#define SPARSE_MAPPING_BRISK_IMAGE_DATABASE_H_
+
+#include <sparse_map/FBrisk.h>
+#include <sparse_map/templated_image_database.h>
+
+namespace sparse_mapping {
+using BriskImageDatabase = TemplatedImageDatabase<DBoW2::FBrisk::TDescriptor, DBoW2::FBrisk>;
+}  // namespace sparse_mapping
+#endif  // SPARSE_MAPPING_BRISK_IMAGE_DATABASE_H_

localization/sparse_mapping/include/sparse_mapping/image_database.h

@@ -0,0 +1,35 @@
+/* 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.
+ */
+
+#ifndef SPARSE_MAPPING_IMAGE_DATABASE_H_
+#define SPARSE_MAPPING_IMAGE_DATABASE_H_
+
+#include <google/protobuf/io/zero_copy_stream_impl.h>
+
+#include <vector>
+
+namespace sparse_mapping {
+class ImageDatabase {
+ public:
+  virtual ~ImageDatabase() {}
+  virtual std::vector<int> Query(const Descriptors& descriptors, const int max_results) = 0;
+  virtual void SaveProtobuf(google::protobuf::io::ZeroCopyOutputStream* output) const = 0;
+  virtual void LoadProtobuf(google::protobuf::io::ZeroCopyInputStream* input, int db_type) = 0;
+};
+}  // namespace sparse_mapping
+#endif  // SPARSE_MAPPING_IMAGE_DATABASE_H_

localization/sparse_mapping/include/sparse_mapping/surf_image_database.h

@@ -0,0 +1,28 @@
+/* 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.
+ */
+
+#ifndef SPARSE_MAPPING_SURF_IMAGE_DATABASE_H_
+#define SPARSE_MAPPING_SURF_IMAGE_DATABASE_H_
+
+#include <sparse_map/templated_image_database.h>
+
+namespace sparse_mapping {
+// TODO(rsoussan): Make sure we use surf 64!!!
+using SurfImageDatabase = TemplatedImageDatabase<DBoW2::FSurf64::TDescriptor, DBoW2::FSurf64>;
+}  // namespace sparse_mapping
+#endif  // SPARSE_MAPPING_SURF_IMAGE_DATABASE_H_

localization/sparse_mapping/include/sparse_mapping/templated_image_database.h

@@ -0,0 +1,81 @@
+/* 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.
+ */
+
+#ifndef SPARSE_MAPPING_TEMPLATED_IMAGE_DATABASE_H_
+#define SPARSE_MAPPING_TEMPLATED_IMAGE_DATABASE_H_
+
+#include <sparse_map/image_database.h>
+#include <sparse_map/image_database_params.h>
+#include <sparse_map/templated_feature_vocabulary.h>
+
+#pragma GCC diagnostic ignored "-Wdelete-non-virtual-dtor"
+#pragma GCC diagnostic push
+#include <DBoW2/DBoW2.h>
+#pragma GCC diagnostic pop
+
+#include <vector>
+#include <string>
+
+namespace sparse_mapping {
+
+template <class TDescriptor, class F>
+class TemplatedImageDatabase : public DBoW2::TemplatedDatabase<TDescriptor, F>, public ImageDatabase {
+ public:
+  TemplatedImageDatabase(const TemplatedFeatureVocabulary<TDescriptor, F>& voc, const ImageDatabaseParams& params);
+  TemplatedImageDatabase(const DescriptorsSet& descriptors_set, const ImageDatabaseParams& params);
+  // Return the cids of the images which are most similar to the current image in sorted order
+  // beginning with the best matching cids
+  std::vector<int> Query(const Descriptors& descriptors, const int max_results) const override;
+
+  // Protobuf Functions
+  explicit TemplatedImageDatabase(google::protobuf::io::ZeroCopyInputStream* input);
+  void SaveProtobuf(google::protobuf::io::ZeroCopyOutputStream* output) const override;
+  void LoadProtobuf(google::protobuf::io::ZeroCopyInputStream* input) override;
+};
+
+// Implementation
+template <class TDescriptor, class F>
+TemplatedImageDatabase<TDescriptor, F>::TemplatedImageDatabase(
+  TemplatedFeatureVocabulary<TDescriptor, F> const& vocabulary, const ImageDatabaseParams& params)
+    : DBoW2::TemplatedDatabase<TDescriptor, F>(vocabulary, params.use_direct_index, params.direct_index_levels) {}
+
+template <class TDescriptor, class F>
+TemplatedImageDatabase<TDescriptor, F>::TemplatedImageDatabase(const DescriptorsSet& descriptors_set,
+                                                               const ImageDatabaseParams& params)
+    : DBoW2::TemplatedDatabase<TDescriptor, F>(params.use_direct_index, params.direct_index_levels) {
+  const TemplatedFeatureVocabulary<TDescriptor, F> vocabulary(descriptors_set, params.vocabulary);
+  setVocabulary(vocabulary);
+  for (const auto& descriptors : descriptors_set) {
+    add(descriptors);
+  }
+}
+
+template <class TDescriptor, class F>
+std::vector<int> TemplatedImageDatabase<TDescriptor, F>::Query(const Descriptors& descriptors, const int max_results) {
+  std::vector<int> matching_cids;
+  DBoW2::QueryResults results;
+  this->query(descriptors, results, max_results);
+  for (const auto& result : results) {
+    matching_cids.push_back(result.Id);
+  }
+  return matching_cids;
+}
+}  // namespace sparse_mapping
+#endif  // SPARSE_MAPPING_TEMPLATED_IMAGE_DATABASE_H_
+
+#include <sparse_mapping/templated_image_database_protobuf.h>

localization/sparse_mapping/include/sparse_mapping/templated_image_database_protobuf.h

@@ -0,0 +1,94 @@
+/* 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.
+ */
+
+#ifndef SPARSE_MAPPING_TEMPLATED_IMAGE_DATABASE_PROTOBUF_H_
+#define SPARSE_MAPPING_TEMPLATED_IMAGE_DATABASE_PROTOBUF_H_
+
+#include <sparse_map.pb.h>
+#include <google/protobuf/io/zero_copy_stream_impl.h>
+
+// Implementation
+template <class TDescriptor, class F>
+TemplatedImageDatabase<TDescriptor, F>::TemplatedImageDatabase(google::protobuf::io::ZeroCopyInputStream* input)
+    : DBoW2::TemplatedDatabase<TDescriptor, F>() {
+  LoadProtobuf(input);
+}
+
+template <class TDescriptor, class F>
+void TemplatedImageDatabase<TDescriptor, F>::LoadProtobuf(google::protobuf::io::ZeroCopyInputStream* input) {
+  TemplatedFeatureVocabulary<TDescriptor, F>* voc = new TemplatedFeatureVocabulary<TDescriptor, F>();
+  voc->LoadProtobuf(input);
+  this->m_voc = voc;
+
+  sparse_mapping_protobuf::DBoWDB db;
+
+  if (!ReadProtobufFrom(input, &db)) {
+    LOG(FATAL) << "Failed to parse db file.";
+  }
+
+  this->clear();  // resizes inverted file
+
+  this->m_nentries = db.num_entries();
+  this->m_use_di = 0;
+  this->m_dilevels = 0;
+
+  for (int i = 0; i < db.num_inverted_index(); ++i) {
+    sparse_mapping_protobuf::DBoWInvertedIndexEntry entry;
+    if (!ReadProtobufFrom(input, &entry)) {
+      LOG(FATAL) << "Failed to parse index entry.";
+    }
+    DBoW2::WordId wid = entry.word_id();
+    DBoW2::EntryId eid = entry.entry_id();
+    DBoW2::WordValue v = entry.weight();
+
+    this->m_ifile[wid].push_back(typename DBoW2::TemplatedDatabase<TDescriptor, F>::IFPair(eid, v));
+  }
+}
+
+template <class TDescriptor, class F>
+void TemplatedImageDatabase<TDescriptor, F>::SaveProtobuf(google::protobuf::io::ZeroCopyOutputStream* output) const {
+  (dynamic_cast<TemplatedFeatureVocabulary<TDescriptor, F>*>(this->m_voc))->SaveProtobuf(output);
+
+  sparse_mapping_protobuf::DBoWDB db;
+
+  db.set_num_entries(this->m_nentries);
+
+  int num_inverted_index = 0;
+  typename DBoW2::TemplatedDatabase<TDescriptor, F>::InvertedFile::const_iterator iit;
+  for (iit = this->m_ifile.begin(); iit != this->m_ifile.end(); ++iit) num_inverted_index += (*iit).size();
+  db.set_num_inverted_index(num_inverted_index);
+  if (!WriteProtobufTo(db, output)) {
+    LOG(FATAL) << "Failed to write db to file.";
+  }
+  typename DBoW2::TemplatedDatabase<TDescriptor, F>::IFRow::const_iterator irit;
+  int word_id = 0;
+  for (iit = this->m_ifile.begin(); iit != this->m_ifile.end(); ++iit) {
+    for (irit = iit->begin(); irit != iit->end(); ++irit) {
+      sparse_mapping_protobuf::DBoWInvertedIndexEntry index;
+      index.set_word_id(word_id);
+      index.set_entry_id(irit->entry_id);
+      index.set_weight(irit->word_weight);
+      if (!WriteProtobufTo(index, output)) {
+        LOG(FATAL) << "Failed to write db index entry to file.";
+      }
+    }
+    word_id++;
+  }
+}
+}  // namespace sparse_mapping
+#endif  // SPARSE_MAPPING_TEMPLATED_IMAGE_DATABASE_PROTOBUF_H_

localization/sparse_mapping/include/sparse_mapping/vocab_tree.h

@@ -76,6 +76,7 @@ namespace sparse_mapping {
     // - DBoW2 binary descriptors (e.g., BRISK, BRIEF)
     // Only one of these is active at one time.
     BinaryDB  * binary_db;
+    FloatDB   * float_db;
 
     int m_num_nodes;
     VocabDB();

localization/sparse_mapping/scripts/copy_bag_topics.py

@@ -0,0 +1,13 @@
+import rosbag
+import sys
+
+if len(sys.argv) < 3:
+    print("Usage: python copy_bag_topics.py <input_bag> <output_bag>")
+    sys.exit(1)
+
+input_bag = sys.argv[1]
+output_bag = sys.argv[2]
+
+with rosbag.Bag(output_bag, 'a') as outbag:
+    for topic, msg, t in rosbag.Bag(input_bag).read_messages():
+        outbag.write(topic, msg, t)