rebased the code to the up-to-date version

src/localization/database/cost_matrix.cpp

@@ -41,7 +41,7 @@ CostMatrix::CostMatrix(const std::string &queryFeaturesDir,
       const auto refFeature = createFeature(type, refFile);
       row.push_back(queryFeature->computeSimilarityScore(*refFeature));
     }
-    LOG(INFO) << "Created row for query image " << fileIdx;
+    LOG(INFO) << "Computed row values for query image " << fileIdx;
     costs_.push_back(row);
   }
   rows_ = costs_.size();
@@ -89,9 +89,11 @@ double CostMatrix::getInverseCost(int row, int col) const {
   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);
 }

src/localization/database/cost_matrix.h

@@ -23,7 +23,7 @@ class CostMatrix {
   void loadFromTxt(const std::string &filename, int rows, int cols);
 
   // TODO(olga): This should be removed once ifeature doesn't have explicit
-  // simlarity score requirement.
+  // similarity score requirement.
   void inverseCosts(bool inverse = true) { inverseCosts_ = inverse; }
 
   void loadFromProto(const std::string &filename);

src/localization/features/scan_context.cpp

@@ -25,8 +25,6 @@ double computeDistanceBetweenGrids(const Eigen::MatrixXd &lhs,
   CHECK(lhs.cols() == rhs.cols()) << "The number of columns is not the same.";
   CHECK(lhs.rows() == rhs.rows()) << "The number of rows is not the same.";
 
-  std::cout << "Rows " << lhs.rows() << " " << rhs.rows() << std::endl;
-
   Eigen::VectorXd columnErrors(lhs.cols());
   for (int c = 0; c < lhs.cols(); ++c) {
     columnErrors[c] = cosine_distance(lhs.col(c), rhs.col(c));

src/python/convert_numpy_to_scan_context.py

@@ -0,0 +1,95 @@
+import numpy as np
+from pathlib import Path
+import argparse
+import protos_io
+
+import protos.localization_protos_pb2 as loc_protos
+
+
+def convert_to_protos(features):
+    """Converts the numpy nd array to the features protos.
+
+    Args:
+        features (numpy.ndarray): feature vectors to be converted.
+                                Expected size NxSxRxC, where N is
+                                number of features and S is number
+                                of shifts, R number of rows and C number of columns.
+    Returns:
+        [image_sequence_localizer.ScanContext]: list of feature protos.
+                                            For details check
+                                            localization_protos.proto file.
+    """
+    protos = []
+    print("Number of features", features.shape)
+    assert (
+        len(features.shape) == 4
+    ), "Expected a N x shifts x rows x cols dimensional matrix."
+    for feature in features:
+        scanContext_proto = loc_protos.ScanContext()
+        for shift in feature:
+            grid_proto = loc_protos.ScanContext.Grid()
+            grid_proto.rows = shift.shape[0]
+            grid_proto.cols = shift.shape[1]
+            for r in range(grid_proto.rows):
+                for c in range(grid_proto.cols):
+                    grid_proto.values.extend([shift[r][c]])
+            scanContext_proto.grids.extend([grid_proto])
+        protos.append(scanContext_proto)
+    return protos
+
+
+def save_protos_to_files(folder, protos, type, prefix):
+    if folder.exists():
+        print("WARNING: the folder exists. Potentially overwritting the files.")
+    else:
+        folder.mkdir()
+    for idx, proto in enumerate(protos):
+        feature_idx = "{0:07d}".format(idx)
+        proto_file = "{prefix}_{feature_idx}.{type}.Feature.pb".format(
+            prefix=prefix, feature_idx=feature_idx, type=type
+        )
+        protos_io.write_feature(folder / proto_file, proto)
+        print("Feature", idx, "was written to ", folder / proto_file)
+    return
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument(
+        "--filename",
+        required=True,
+        type=Path,
+        help="Path to file that contains features that can be loaded with np.load()",
+    )
+    parser.add_argument(
+        "--feature_type",
+        required=True,
+        type=str,
+        help="Type of the features, e.g. ScanContext",
+    )
+    parser.add_argument(
+        "--output_folder", required=True, type=Path, help="Path to output directory"
+    )
+    parser.add_argument(
+        "--output_file_prefix",
+        required=False,
+        type=str,
+        default="scanContext",
+        help="Prefix for every feature file that will be generated",
+    )
+
+    args = parser.parse_args()
+    try:
+        features = np.load(args.filename)
+    except:
+        print("ERROR: Could not read features from", args.filename)
+        return
+    print("There are: ", len(features))
+    protos = convert_to_protos(features)
+    save_protos_to_files(
+        args.output_folder, protos, args.feature_type, args.output_file_prefix
+    )
+
+
+if __name__ == "__main__":
+    main()

src/python/run_scan_context_matching.py

@@ -77,7 +77,7 @@ def runResultVisualization(config):
         image_name=config["matchingResultImage"]
     )
 
-    params += "--expanded_patches_file {file} ".format(file=config["expandedNodesFile"])
+    # params += "--expanded_patches_file {file} ".format(file=config["expandedNodesFile"])
     command = "python visualize_localization_result.py " + params
     print("Calling:", command)
     os.system(command)