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);
  }
 
@@ -289,6 +301,9 @@ namespace interest_point {
  } else {
  // Traditional floating point descriptor
  cv::FlannBasedMatcher matcher;
+ if (img1_descriptor_map.rows < 2 || // at least two are needed for knnMatch, otherwise error
+ img2_descriptor_map.rows < 2)
+ return;
  std::vector<std::vector<cv::DMatch> > possible_matches;
  matcher.knnMatch(img1_descriptor_map, img2_descriptor_map, possible_matches, 2);
  matches->clear();

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)