Adding ScanContext support

.github/workflows/main.yml

@@ -28,7 +28,7 @@ jobs:
       run: sudo apt update
 
     - name: Install dependencies
-      run: sudo apt install -y libopencv-dev libyaml-cpp-dev libprotobuf-dev libprotoc-dev protobuf-compiler
+      run: sudo apt install -y libopencv-dev libyaml-cpp-dev libprotobuf-dev libprotoc-dev protobuf-compiler libeigen3-dev
 
     - name: Build
       run: |

readme.md

@@ -13,7 +13,7 @@ The assumption is then that the input is a sequence of images. The program outpu
 
 Prerequisites:
 ```
-sudo apt-get install -y build-essential libopencv-dev libyaml-cpp-dev libprotobuf-dev libprotoc-dev protobuf-compiler
+sudo apt-get install -y libopencv-dev libyaml-cpp-dev libprotobuf-dev libprotoc-dev protobuf-compiler libeigen3-dev
 ```
 Tested on Ubuntu 20.04.
 
@@ -56,6 +56,17 @@ For more details about the underlying method and the interpretation of the resul
 Here is a sketch of what roughly is happening for those who don't like to read much ![](doc/cost_matrix_view.png)
 
 
+## LIDAR place recognition
+
+The code allows to match also the ScanContext sequences.
+To do place recognition in this case, you need for now 3 steps:
+1. Convert numpy arrays into protobuf, use `python/convert_numpy_to_scan_context.py`
+2. Compute scan context cost matrix, use `./src/apps/cost_matrix_based_matching/compute_scan_context_cost_matrix queryFeaturesDir referenceFeaturesDir outputFilename` from `build` folder
+3. Run online place recognition with pre computed cost matrix, use `python/run_scan_context_matching.py`
+
+
+More general support will come later.
+
 ## Parent project
 
 This repository is a continuation of my previous works [vpr_relocalization](https://github.com/PRBonn/vpr_relocalization) and [online_place_recognition](https://github.com/PRBonn/online_place_recognition).

src/apps/cost_matrix_based_matching/CMakeLists.txt

@@ -37,3 +37,20 @@ target_link_libraries(localization_by_hashing
     ${OpenCV_LIBS} 
 )
 
+add_executable(scan_context_matching scan_context_matching.cpp)
+target_link_libraries(scan_context_matching
+    cost_matrix 
+    config_parser
+    online_database
+    successor_manager
+    online_localizer
+    scan_context_relocalizer 
+    glog::glog
+)
+
+add_executable(compute_scan_context_cost_matrix compute_scan_context_cost_matrix.cpp)
+target_link_libraries(compute_scan_context_cost_matrix
+    cost_matrix 
+    glog::glog
+)
+

src/apps/cost_matrix_based_matching/compute_scan_context_cost_matrix.cpp

@@ -0,0 +1,35 @@
+/* By O. Vysotska in 2023 */
+
+#include "database/cost_matrix.h"
+#include "database/cost_matrix_database.h"
+#include "features/feature_factory.h"
+#include "tools/config_parser/config_parser.h"
+
+#include <glog/logging.h>
+
+#include <memory>
+
+int main(int argc, char *argv[]) {
+  // TODO(olga) Add gflags support.
+  google::InitGoogleLogging(argv[0]);
+  FLAGS_logtostderr = 1;
+  LOG(INFO) << "===== Compute Cost Matrix for Scan Context ====\n";
+
+  if (argc < 4) {
+    LOG(ERROR) << "Not enough input parameters.";
+    LOG(INFO) << "Proper usage: ./compute_scan_context_cost_matrix "
+                 "queryFeaturesDir referenceFeaturesDir outputFilename";
+    exit(0);
+  }
+
+  std::string queryFeaturesDir = argv[1];
+  std::string refFeaturesDir = argv[2];
+  std::string outputFilename = argv[3];
+
+  const localization::database::CostMatrix costMatrix(
+      queryFeaturesDir, refFeaturesDir, localization::features::Scan_Context);
+  costMatrix.storeToProto(outputFilename);
+
+  LOG(INFO) << "Cost matrix is saved to" << outputFilename;
+  return 0;
+}

src/apps/cost_matrix_based_matching/online_localizer_lsh.cpp

@@ -63,7 +63,7 @@ int main(int argc, char *argv[]) {
 
   if (argc < 2) {
     printf("[ERROR] Not enough input parameters.\n");
-    printf("Proper usage: ./cost_matrix_based_matching_lsh config_file.yaml\n");
+    printf("Proper usage: ./online_localizer_lsh config_file.yaml\n");
     exit(0);
   }
 
@@ -77,7 +77,7 @@ int main(int argc, char *argv[]) {
       /*refFeaturesDir=*/parser.path2ref,
       /*type=*/loc::features::FeatureType::Cnn_Feature,
       /*bufferSize=*/parser.bufferSize,
-      /*costMatrixFile=*/parser.costMatrix);
+      /*costMatrixFile=*/parser.costMatrix, /*invert=*/true);
 
   auto relocalizer = std::make_unique<loc::relocalizers::LshCvHashing>(
       /*onlineDatabase=*/database.get(),

src/apps/cost_matrix_based_matching/scan_context_matching.cpp

@@ -0,0 +1,55 @@
+/* By O. Vysotska in 2023 */
+
+#include "database/cost_matrix.h"
+#include "database/online_database.h"
+#include "features/feature_factory.h"
+#include "online_localizer/online_localizer.h"
+#include "relocalizers/scan_context_relocalizer.h"
+#include "successor_manager/successor_manager.h"
+#include "tools/config_parser/config_parser.h"
+
+#include <glog/logging.h>
+
+#include <filesystem>
+#include <memory>
+
+namespace loc = localization;
+
+int main(int argc, char *argv[]) {
+  google::InitGoogleLogging(argv[0]);
+  FLAGS_logtostderr = 1;
+  LOG(INFO) << "===== Matching ScanContext sequences ====\n";
+
+  if (argc < 2) {
+    LOG(ERROR) << "Not enough input parameters.";
+    LOG(INFO) << "Proper usage: ./scan_context_matching "
+                 "config_file.yaml";
+    exit(0);
+  }
+
+  std::string config_file = argv[1];
+  ConfigParser parser;
+  parser.parseYaml(config_file);
+  parser.print();
+
+  const auto database = std::make_unique<loc::database::OnlineDatabase>(
+      parser.path2qu, parser.path2ref, loc::features::FeatureType::Scan_Context,
+      parser.bufferSize, parser.costMatrix, false);
+
+  const auto relocalizer =
+      std::make_unique<loc::relocalizers::ScanContextRelocalizer>(
+          parser.path2ref, database.get());
+
+  const auto successorManager =
+      std::make_unique<loc::successor_manager::SuccessorManager>(
+          database.get(), relocalizer.get(), parser.fanOut);
+  loc::online_localizer::OnlineLocalizer localizer{
+      successorManager.get(), parser.expansionRate, parser.nonMatchCost};
+  const loc::online_localizer::Matches imageMatches =
+      localizer.findMatchesTill(parser.querySize);
+  loc::online_localizer::storeMatchesAsProto(imageMatches,
+                                             parser.matchingResult);
+
+  LOG(INFO) << "Done.";
+  return 0;
+}

src/localization/database/cost_matrix.cpp

@@ -28,18 +28,20 @@ CostMatrix::CostMatrix(const std::string &queryFeaturesDir,
   const std::vector<std::string> refFeaturesFiles =
       listProtoDir(refFeaturesDir, ".Feature");
 
-  std::cerr << "Query features" << queryFeaturesFiles.size() << std::endl;
-  std::cerr << "ref features" << refFeaturesFiles.size() << std::endl;
+  LOG(INFO) << "Query features size " << queryFeaturesFiles.size();
+  LOG(INFO) << "Reference features size " << refFeaturesDir.size();
+  LOG(INFO) << "Computing cost matrix. This make take some time...";
 
   costs_.reserve(queryFeaturesFiles.size());
-  for (const auto &queryFile : queryFeaturesFiles) {
-    auto queryFeature = createFeature(type, queryFile);
+  for (int fileIdx = 0; fileIdx < queryFeaturesFiles.size(); ++fileIdx) {
+    auto queryFeature = createFeature(type, queryFeaturesFiles[fileIdx]);
     std::vector<double> row;
     row.reserve(refFeaturesFiles.size());
     for (const auto &refFile : refFeaturesFiles) {
       const auto refFeature = createFeature(type, refFile);
       row.push_back(queryFeature->computeSimilarityScore(*refFeature));
     }
+    LOG(INFO) << "Computed row values for query image " << fileIdx;
     costs_.push_back(row);
   }
   rows_ = costs_.size();
@@ -76,17 +78,22 @@ void CostMatrix::loadFromTxt(const std::string &filename, int rows, int cols) {
 double CostMatrix::at(int row, int col) const {
   CHECK(row >= 0 && row < rows_) << "Row outside range " << row;
   CHECK(col >= 0 && col < cols_) << "Col outside range " << col;
+  if (inverseCosts_) {
+    return getInverseCost(row, col);
+  }
   return costs_[row][col];
 }
 
 double CostMatrix::getInverseCost(int row, int col) const {
-  const double value = this->at(row, col);
+  const double value = costs_[row][col];
   if (std::abs(value) < kEpsilon) {
     return std::numeric_limits<double>::max();
   }
-  if (value < 0){
-    LOG(WARNING) << "The cost value for row:" << row  << " and col:" << col <<" is < 0: " << value<< ". This should not be like this. I will make a positive value of it for now. But please check your values";
-
+  if (value < 0) {
+    LOG(WARNING) << "The cost value for row:" << row << " and col:" << col
+                 << " is < 0: " << value
+                 << ". This should not be like this. I will make a positive "
+                    "value of it for now. But please check your values";
   }
   return 1. / std::abs(value);
 }
@@ -112,4 +119,25 @@ void CostMatrix::loadFromProto(const std::string &filename) {
   LOG(INFO) << "Read cost matrix with " << rows_ << " rows and " << cols_
             << " cols.";
 }
+
+void CostMatrix::storeToProto(const std::string &protoFilename) const {
+  image_sequence_localizer::CostMatrix costMatrixProto;
+  costMatrixProto.set_cols(cols_);
+  costMatrixProto.set_rows(rows_);
+  for (int r = 0; r < rows_; ++r) {
+    for (int c = 0; c < cols_; ++c) {
+      costMatrixProto.add_values(costs_[r][c]);
+    }
+  }
+
+  std::fstream out(protoFilename,
+                   std::ios::out | std::ios::trunc | std::ios::binary);
+  if (!costMatrixProto.SerializeToOstream(&out)) {
+    LOG(ERROR) << "Couldn't open the file " << protoFilename;
+    LOG(ERROR) << "The path is NOT saved.";
+    return;
+  }
+  out.close();
+  LOG(INFO) << "The cost matrix was written to " << protoFilename;
+}
 } // namespace localization::database

src/localization/database/cost_matrix.h

@@ -9,6 +9,7 @@
 #include <vector>
 
 namespace localization::database {
+
 class CostMatrix {
 public:
   using Matrix = std::vector<std::vector<double>>;
@@ -21,19 +22,26 @@ class CostMatrix {
 
   void loadFromTxt(const std::string &filename, int rows, int cols);
 
+  // TODO(olga): This should be removed once ifeature doesn't have explicit
+  // similarity score requirement.
+  void inverseCosts(bool inverse = true) { inverseCosts_ = inverse; }
+
   void loadFromProto(const std::string &filename);
+  void storeToProto(const std::string &protoFilename) const;
   const Matrix &getCosts() const { return costs_; }
 
   double at(int row, int col) const;
   // Computes 1/value.
   double getInverseCost(int row, int col) const;
+
   int rows() const { return rows_; }
   int cols() const { return cols_; }
 
 private:
   Matrix costs_;
   int rows_ = 0;
   int cols_ = 0;
+  bool inverseCosts_ = true;
 };
 } // namespace localization::database
 

src/localization/database/cost_matrix_database.cpp

@@ -37,7 +37,7 @@ CostMatrixDatabase::CostMatrixDatabase(const std::string &costMatrixFile)
     : costMatrix_(CostMatrix(costMatrixFile)) {}
 
 double CostMatrixDatabase::getCost(int quId, int refId) {
-  return costMatrix_.getInverseCost(quId, refId);
+  return costMatrix_.at(quId, refId);
 }
 
 } // namespace localization::database

src/localization/database/online_database.cpp

@@ -35,6 +35,7 @@
 #include <memory>
 #include <string>
 
+#include <iostream>
 namespace localization::database {
 
 namespace {
@@ -54,7 +55,7 @@ addFeatureIfNeeded(features::FeatureBuffer &featureBuffer,
 OnlineDatabase::OnlineDatabase(const std::string &queryFeaturesDir,
                                const std::string &refFeaturesDir,
                                features::FeatureType type, int bufferSize,
-                               const std::string &costMatrixFile)
+                               const std::string &costMatrixFile, bool invert)
     : quFeaturesNames_{listProtoDir(queryFeaturesDir, ".Feature")},
       refFeaturesNames_{listProtoDir(refFeaturesDir, ".Feature")},
       featureType_{type}, refBuffer_{std::make_unique<features::FeatureBuffer>(
@@ -64,6 +65,7 @@ OnlineDatabase::OnlineDatabase(const std::string &queryFeaturesDir,
   LOG_IF(FATAL, refFeaturesNames_.empty()) << "Reference features are not set.";
   if (!costMatrixFile.empty()) {
     precomputedCosts_ = CostMatrix(costMatrixFile);
+    precomputedCosts_->inverseCosts(invert);
   }
 }
 
@@ -83,7 +85,7 @@ double OnlineDatabase::computeMatchingCost(int quId, int refId) {
 
 double OnlineDatabase::getCost(int quId, int refId) {
   if (precomputedCosts_) {
-    return precomputedCosts_->getInverseCost(quId, refId);
+    return precomputedCosts_->at(quId, refId);
   }
   // Check if the cost was computed before.
   auto rowIter = costs_.find(quId);

src/localization/database/online_database.h

@@ -46,7 +46,8 @@ class OnlineDatabase : public iDatabase {
 public:
   OnlineDatabase(const std::string &queryFeaturesDir,
                  const std::string &refFeaturesDir, features::FeatureType type,
-                 int bufferSize, const std::string &costMatrixFile = "");
+                 int bufferSize, const std::string &costMatrixFile = "",
+                 bool invert = true);
 
   inline int refSize() override { return refFeaturesNames_.size(); }
   double getCost(int quId, int refId) override;

src/localization/features/CMakeLists.txt

@@ -5,12 +5,26 @@ target_link_libraries(cnn_feature
     glog::glog
 )
 
+find_package (Eigen3 3.3 REQUIRED NO_MODULE)
+
+add_library(scan_context scan_context.cpp)
+target_link_libraries(scan_context 
+    glog::glog
+    cxx_flags
+    Eigen3::Eigen
+)
+
 add_library(feature_factory feature_factory.cpp)
 target_link_libraries(feature_factory 
     PUBLIC 
-    cnn_feature 
+    cnn_feature
+    scan_context 
     glog::glog
+
 )
 
 add_library(feature_buffer feature_buffer.cpp)
 target_link_libraries(feature_buffer glog::glog cxx_flags)
+
+
+

src/localization/features/cnn_feature.h

@@ -50,7 +50,7 @@ class CnnFeature : public iFeature {
    */
   double score2cost(double score) const override;
 
-  using iFeature::bits;
+  // using iFeature::bits;
   using iFeature::dimensions;
 
 protected:

src/localization/features/feature_factory.cpp

@@ -23,8 +23,10 @@
 
 #include "feature_factory.h"
 #include "cnn_feature.h"
+#include "features/scan_context.h"
 
 #include <glog/logging.h>
+#include <memory>
 
 namespace localization::features {
 
@@ -34,6 +36,9 @@ std::unique_ptr<iFeature> createFeature(FeatureType type,
   case Cnn_Feature: {
     return std::make_unique<CnnFeature>(featureFilename);
   }
+  case Scan_Context: {
+    return std::make_unique<ScanContext>(featureFilename);
+  }
   }
   LOG(FATAL) << "Unknown feature type";
 }