diff --git a/.gitignore b/.gitignore index 82f212e91b..942f4db9d3 100644 --- a/.gitignore +++ b/.gitignore @@ -98,3 +98,8 @@ tags .ctags .vscode + +# submodules/ +tools/localization_analysis/scripts/ +localization/sparse_mapping/scripts/ +localization/sparse_mapping/tools/ \ No newline at end of file diff --git a/localization/interest_point/src/matching.cc b/localization/interest_point/src/matching.cc index 37ea1ab808..3b421071b4 100644 --- a/localization/interest_point/src/matching.cc +++ b/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 > possible_matches; matcher.knnMatch(img1_descriptor_map, img2_descriptor_map, possible_matches, 2); matches->clear(); diff --git a/localization/localization_node/src/localization.cc b/localization/localization_node/src/localization.cc index 1f8a0f7f7b..1e31f1246a 100644 --- a/localization/localization_node/src/localization.cc +++ b/localization/localization_node/src/localization.cc @@ -100,7 +100,7 @@ bool Localizer::Localize(cv_bridge::CvImageConstPtr image_ptr, ff_msgs::VisualLa std::vector 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; } diff --git a/localization/sparse_mapping/include/sparse_mapping/brisk_image_database.h b/localization/sparse_mapping/include/sparse_mapping/brisk_image_database.h new file mode 100644 index 0000000000..74f3348726 --- /dev/null +++ b/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 +#include + +namespace sparse_mapping { +using BriskImageDatabase = TemplatedImageDatabase; +} // namespace sparse_mapping +#endif // SPARSE_MAPPING_BRISK_IMAGE_DATABASE_H_ diff --git a/localization/sparse_mapping/include/sparse_mapping/image_database.h b/localization/sparse_mapping/include/sparse_mapping/image_database.h new file mode 100644 index 0000000000..02dd527cd1 --- /dev/null +++ b/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 + +#include + +namespace sparse_mapping { +class ImageDatabase { + public: + virtual ~ImageDatabase() {} + virtual std::vector 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_ diff --git a/localization/sparse_mapping/include/sparse_mapping/surf_image_database.h b/localization/sparse_mapping/include/sparse_mapping/surf_image_database.h new file mode 100644 index 0000000000..d3cfc92f76 --- /dev/null +++ b/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 + +namespace sparse_mapping { +// TODO(rsoussan): Make sure we use surf 64!!! +using SurfImageDatabase = TemplatedImageDatabase; +} // namespace sparse_mapping +#endif // SPARSE_MAPPING_SURF_IMAGE_DATABASE_H_ diff --git a/localization/sparse_mapping/include/sparse_mapping/templated_image_database.h b/localization/sparse_mapping/include/sparse_mapping/templated_image_database.h new file mode 100644 index 0000000000..7f448030aa --- /dev/null +++ b/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 +#include +#include + +#pragma GCC diagnostic ignored "-Wdelete-non-virtual-dtor" +#pragma GCC diagnostic push +#include +#pragma GCC diagnostic pop + +#include +#include + +namespace sparse_mapping { + +template +class TemplatedImageDatabase : public DBoW2::TemplatedDatabase, public ImageDatabase { + public: + TemplatedImageDatabase(const TemplatedFeatureVocabulary& 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 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 +TemplatedImageDatabase::TemplatedImageDatabase( + TemplatedFeatureVocabulary const& vocabulary, const ImageDatabaseParams& params) + : DBoW2::TemplatedDatabase(vocabulary, params.use_direct_index, params.direct_index_levels) {} + +template +TemplatedImageDatabase::TemplatedImageDatabase(const DescriptorsSet& descriptors_set, + const ImageDatabaseParams& params) + : DBoW2::TemplatedDatabase(params.use_direct_index, params.direct_index_levels) { + const TemplatedFeatureVocabulary vocabulary(descriptors_set, params.vocabulary); + setVocabulary(vocabulary); + for (const auto& descriptors : descriptors_set) { + add(descriptors); + } +} + +template +std::vector TemplatedImageDatabase::Query(const Descriptors& descriptors, const int max_results) { + std::vector 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 diff --git a/localization/sparse_mapping/include/sparse_mapping/templated_image_database_protobuf.h b/localization/sparse_mapping/include/sparse_mapping/templated_image_database_protobuf.h new file mode 100644 index 0000000000..cdaee21013 --- /dev/null +++ b/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 +#include + +// Implementation +template +TemplatedImageDatabase::TemplatedImageDatabase(google::protobuf::io::ZeroCopyInputStream* input) + : DBoW2::TemplatedDatabase() { + LoadProtobuf(input); +} + +template +void TemplatedImageDatabase::LoadProtobuf(google::protobuf::io::ZeroCopyInputStream* input) { + TemplatedFeatureVocabulary* voc = new TemplatedFeatureVocabulary(); + 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::IFPair(eid, v)); + } +} + +template +void TemplatedImageDatabase::SaveProtobuf(google::protobuf::io::ZeroCopyOutputStream* output) const { + (dynamic_cast*>(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::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::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_ diff --git a/localization/sparse_mapping/include/sparse_mapping/vocab_tree.h b/localization/sparse_mapping/include/sparse_mapping/vocab_tree.h index 6f6782d82d..d3c54dd2c8 100644 --- a/localization/sparse_mapping/include/sparse_mapping/vocab_tree.h +++ b/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(); diff --git a/localization/sparse_mapping/scripts/copy_bag_topics.py b/localization/sparse_mapping/scripts/copy_bag_topics.py new file mode 100644 index 0000000000..8836b61df9 --- /dev/null +++ b/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 ") + 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) diff --git a/localization/sparse_mapping/scripts/rbo.py b/localization/sparse_mapping/scripts/rbo.py new file mode 100644 index 0000000000..ed90bc11ef --- /dev/null +++ b/localization/sparse_mapping/scripts/rbo.py @@ -0,0 +1,322 @@ +"""Rank-biased overlap, a ragged sorted list similarity measure. + +See http://doi.acm.org/10.1145/1852102.1852106 for details. All functions +directly corresponding to concepts from the paper are named so that they can be +clearly cross-identified. + +The definition of overlap has been modified to account for ties. Without this, +results for lists with tied items were being inflated. The modification itself +is not mentioned in the paper but seems to be reasonable, see function +``overlap()``. Places in the code which diverge from the spec in the paper +because of this are highlighted with comments. + +The two main functions for performing an RBO analysis are ``rbo()`` and +``rbo_dict()``; see their respective docstrings for how to use them. + +The following doctest just checks that equivalent specifications of a +problem yield the same result using both functions: + + >>> lst1 = [{"c", "a"}, "b", "d"] + >>> lst2 = ["a", {"c", "b"}, "d"] + >>> ans_rbo = _round(rbo(lst1, lst2, p=.9)) + >>> dct1 = dict(a=1, b=2, c=1, d=3) + >>> dct2 = dict(a=1, b=2, c=2, d=3) + >>> ans_rbo_dict = _round(rbo_dict(dct1, dct2, p=.9, sort_ascending=True)) + >>> ans_rbo == ans_rbo_dict + True + +""" + +from __future__ import division + +import math +from bisect import bisect_left +from collections import namedtuple + + +RBO = namedtuple("RBO", "min res ext") +RBO.__doc__ += ": Result of full RBO analysis" +RBO.min.__doc__ = "Lower bound estimate" +RBO.res.__doc__ = "Residual corresponding to min; min + res is an upper bound estimate" +RBO.ext.__doc__ = "Extrapolated point estimate" + + +def _round(obj): + if isinstance(obj, RBO): + return RBO(_round(obj.min), _round(obj.res), _round(obj.ext)) + else: + return round(obj, 3) + + +def set_at_depth(lst, depth): + ans = set() + for v in lst[:depth]: + if isinstance(v, set): + ans.update(v) + else: + ans.add(v) + return ans + + +def raw_overlap(list1, list2, depth): + """Overlap as defined in the article. + + """ + set1, set2 = set_at_depth(list1, depth), set_at_depth(list2, depth) + return len(set1.intersection(set2)), len(set1), len(set2) + + +def overlap(list1, list2, depth): + """Overlap which accounts for possible ties. + + This isn't mentioned in the paper but should be used in the ``rbo*()`` + functions below, otherwise overlap at a given depth might be > depth which + inflates the result. + + There are no guidelines in the paper as to what's a good way to calculate + this, but a good guess is agreement scaled by the minimum between the + requested depth and the lengths of the considered lists (overlap shouldn't + be larger than the number of ranks in the shorter list, otherwise results + are conspicuously wrong when the lists are of unequal lengths -- rbo_ext is + not between rbo_min and rbo_min + rbo_res. + + >>> overlap("abcd", "abcd", 3) + 3.0 + + >>> overlap("abcd", "abcd", 5) + 4.0 + + >>> overlap(["a", {"b", "c"}, "d"], ["a", {"b", "c"}, "d"], 2) + 2.0 + + >>> overlap(["a", {"b", "c"}, "d"], ["a", {"b", "c"}, "d"], 3) + 3.0 + + """ + return agreement(list1, list2, depth) * min(depth, len(list1), len(list2)) + # NOTE: comment the preceding and uncomment the following line if you want + # to stick to the algorithm as defined by the paper + # return raw_overlap(list1, list2, depth)[0] + + +def agreement(list1, list2, depth): + """Proportion of shared values between two sorted lists at given depth. + + >>> _round(agreement("abcde", "abdcf", 1)) + 1.0 + >>> _round(agreement("abcde", "abdcf", 3)) + 0.667 + >>> _round(agreement("abcde", "abdcf", 4)) + 1.0 + >>> _round(agreement("abcde", "abdcf", 5)) + 0.8 + >>> _round(agreement([{1, 2}, 3], [1, {2, 3}], 1)) + 0.667 + >>> _round(agreement([{1, 2}, 3], [1, {2, 3}], 2)) + 1.0 + + """ + len_intersection, len_set1, len_set2 = raw_overlap(list1, list2, depth) + return 2 * len_intersection / (len_set1 + len_set2) + + +def cumulative_agreement(list1, list2, depth): + return (agreement(list1, list2, d) for d in range(1, depth + 1)) + + +def average_overlap(list1, list2, depth=None): + """Calculate average overlap between ``list1`` and ``list2``. + + >>> _round(average_overlap("abcdefg", "zcavwxy", 1)) + 0.0 + >>> _round(average_overlap("abcdefg", "zcavwxy", 2)) + 0.0 + >>> _round(average_overlap("abcdefg", "zcavwxy", 3)) + 0.222 + >>> _round(average_overlap("abcdefg", "zcavwxy", 4)) + 0.292 + >>> _round(average_overlap("abcdefg", "zcavwxy", 5)) + 0.313 + >>> _round(average_overlap("abcdefg", "zcavwxy", 6)) + 0.317 + >>> _round(average_overlap("abcdefg", "zcavwxy", 7)) + 0.312 + + """ + depth = min(len(list1), len(list2)) if depth is None else depth + return sum(cumulative_agreement(list1, list2, depth)) / depth + + +def rbo_at_k(list1, list2, p, depth=None): + # ``p**d`` here instead of ``p**(d - 1)`` because enumerate starts at + # 0 + depth = min(len(list1), len(list2)) if depth is None else depth + d_a = enumerate(cumulative_agreement(list1, list2, depth)) + return (1 - p) * sum(p ** d * a for (d, a) in d_a) + + +def rbo_min(list1, list2, p, depth=None): + """Tight lower bound on RBO. + + See equation (11) in paper. + + >>> _round(rbo_min("abcdefg", "abcdefg", .9)) + 0.767 + >>> _round(rbo_min("abcdefgh", "abcdefg", .9)) + 0.767 + + """ + depth = min(len(list1), len(list2)) if depth is None else depth + x_k = overlap(list1, list2, depth) + log_term = x_k * math.log(1 - p) + sum_term = sum( + p ** d / d * (overlap(list1, list2, d) - x_k) for d in range(1, depth + 1) + ) + return (1 - p) / p * (sum_term - log_term) + + +def rbo_res(list1, list2, p): + """Upper bound on residual overlap beyond evaluated depth. + + See equation (30) in paper. + + NOTE: The doctests weren't verified against manual computations but seem + plausible. In particular, for identical lists, ``rbo_min()`` and + ``rbo_res()`` should add up to 1, which is the case. + + >>> _round(rbo_res("abcdefg", "abcdefg", .9)) + 0.233 + >>> _round(rbo_res("abcdefg", "abcdefghijklmnopqrstuvwxyz", .9)) + 0.239 + + """ + S, L = sorted((list1, list2), key=len) + s, l = len(S), len(L) + x_l = overlap(list1, list2, l) + # since overlap(...) can be fractional in the general case of ties and f + # must be an integer --> math.ceil() + f = int(math.ceil(l + s - x_l)) + # upper bound of range() is non-inclusive, therefore + 1 is needed + term1 = s * sum(p ** d / d for d in range(s + 1, f + 1)) + term2 = l * sum(p ** d / d for d in range(l + 1, f + 1)) + term3 = x_l * (math.log(1 / (1 - p)) - sum(p ** d / d for d in range(1, f + 1))) + return p ** s + p ** l - p ** f - (1 - p) / p * (term1 + term2 + term3) + + +def rbo_ext(list1, list2, p): + """RBO point estimate based on extrapolating observed overlap. + + See equation (32) in paper. + + NOTE: The doctests weren't verified against manual computations but seem + plausible. + + >>> _round(rbo_ext("abcdefg", "abcdefg", .9)) + 1.0 + >>> _round(rbo_ext("abcdefg", "bacdefg", .9)) + 0.9 + + """ + S, L = sorted((list1, list2), key=len) + s, l = len(S), len(L) + x_l = overlap(list1, list2, l) + x_s = overlap(list1, list2, s) + # the paper says overlap(..., d) / d, but it should be replaced by + # agreement(..., d) defined as per equation (28) so that ties are handled + # properly (otherwise values > 1 will be returned) + # sum1 = sum(p**d * overlap(list1, list2, d)[0] / d for d in range(1, l + 1)) + sum1 = sum(p ** d * agreement(list1, list2, d) for d in range(1, l + 1)) + sum2 = sum(p ** d * x_s * (d - s) / s / d for d in range(s + 1, l + 1)) + term1 = (1 - p) / p * (sum1 + sum2) + term2 = p ** l * ((x_l - x_s) / l + x_s / s) + return term1 + term2 + + +def rbo(list1, list2, p): + """Complete RBO analysis (lower bound, residual, point estimate). + + ``list`` arguments should be already correctly sorted iterables and each + item should either be an atomic value or a set of values tied for that + rank. ``p`` is the probability of looking for overlap at rank k + 1 after + having examined rank k. + + >>> lst1 = [{"c", "a"}, "b", "d"] + >>> lst2 = ["a", {"c", "b"}, "d"] + >>> _round(rbo(lst1, lst2, p=.9)) + RBO(min=0.489, res=0.477, ext=0.967) + + """ + if not 0 <= p <= 1: + raise ValueError("The ``p`` parameter must be between 0 and 1.") + args = (list1, list2, p) + return RBO(rbo_min(*args), rbo_res(*args), rbo_ext(*args)) + + +def sort_dict(dct, *, ascending=False): + """Sort keys in ``dct`` according to their corresponding values. + + Sorts in descending order by default, because the values are + typically scores, i.e. the higher the better. Specify + ``ascending=True`` if the values are ranks, or some sort of score + where lower values are better. + + Ties are handled by creating sets of tied keys at the given position + in the sorted list. + + >>> dct = dict(a=1, b=2, c=1, d=3) + >>> list(sort_dict(dct)) == ['d', 'b', {'a', 'c'}] + True + >>> list(sort_dict(dct, ascending=True)) == [{'a', 'c'}, 'b', 'd'] + True + + """ + scores = [] + items = [] + # items should be unique, scores don't have to + for item, score in dct.items(): + if not ascending: + score *= -1 + i = bisect_left(scores, score) + if i == len(scores): + scores.append(score) + items.append(item) + elif scores[i] == score: + existing_item = items[i] + if isinstance(existing_item, set): + existing_item.add(item) + else: + items[i] = {existing_item, item} + else: + scores.insert(i, score) + items.insert(i, item) + return items + + +def rbo_dict(dict1, dict2, p, *, sort_ascending=False): + """Wrapper around ``rbo()`` for dict input. + + Each dict maps items to be sorted to the score according to which + they should be sorted. The RBO analysis is then performed on the + resulting sorted lists. + + The sort is descending by default, because scores are typically the + higher the better, but this can be overridden by specifying + ``sort_ascending=True``. + + >>> dct1 = dict(a=1, b=2, c=1, d=3) + >>> dct2 = dict(a=1, b=2, c=2, d=3) + >>> _round(rbo_dict(dct1, dct2, p=.9, sort_ascending=True)) + RBO(min=0.489, res=0.477, ext=0.967) + + """ + list1, list2 = ( + sort_dict(dict1, ascending=sort_ascending), + sort_dict(dict2, ascending=sort_ascending), + ) + return rbo(list1, list2, p) + + +if __name__ in ("__main__", "__console__"): + import doctest + + doctest.testmod() \ No newline at end of file diff --git a/localization/sparse_mapping/src/reprojection.cc b/localization/sparse_mapping/src/reprojection.cc index 5bf243f717..b559ef6c0c 100644 --- a/localization/sparse_mapping/src/reprojection.cc +++ b/localization/sparse_mapping/src/reprojection.cc @@ -397,8 +397,12 @@ int RansacEstimateCamera(const std::vector & landmarks, << best_inliers << " inliers\n"; // TODO(bcoltin): Return some sort of confidence? - if (best_inliers < FLAGS_num_min_localization_inliers) + if (best_inliers < FLAGS_num_min_localization_inliers) { + if (verbose) + std::cout << "num best inliers " << best_inliers << + " num min localization inliers " << FLAGS_num_min_localization_inliers<< std::endl; return 2; + } std::vector inliers; CountInliers(landmarks, observations, *camera_estimate, inlier_tolerance, &inliers); diff --git a/localization/sparse_mapping/src/sparse_map.cc b/localization/sparse_mapping/src/sparse_map.cc index b6fd572b49..340fab73f1 100644 --- a/localization/sparse_mapping/src/sparse_map.cc +++ b/localization/sparse_mapping/src/sparse_map.cc @@ -24,6 +24,7 @@ #include #include #include +#include // change later #include #include @@ -272,8 +273,10 @@ void SparseMap::DetectFeatures() { void SparseMap::Load(const std::string & protobuf_file, bool localization) { sparse_mapping_protobuf::Map map; int input_fd = open(protobuf_file.c_str(), O_RDONLY); - if (input_fd < 0) + if (input_fd < 0) { + std::cout <<" Error code "<< errno << std::endl; LOG(FATAL) << "Failed to open map file: " << protobuf_file; + } google::protobuf::io::ZeroCopyInputStream* input = new google::protobuf::io::FileInputStream(input_fd); @@ -471,6 +474,8 @@ void SparseMap::Save(const std::string & protobuf_file) const { if (vocab_db_.binary_db != NULL) map.set_vocab_db(sparse_mapping_protobuf::Map::BINARYDB); + if (vocab_db_.float_db != NULL) + map.set_vocab_db(sparse_mapping_protobuf::Map::FLOATDB); map.set_histogram_equalization(histogram_equalization_); @@ -548,7 +553,7 @@ void SparseMap::Save(const std::string & protobuf_file) const { LOG(FATAL) << "Failed to write landmark to file."; } - if (vocab_db_.binary_db != NULL) + if (vocab_db_.binary_db != NULL || vocab_db_.float_db != NULL) vocab_db_.SaveProtobuf(output); delete output; @@ -686,6 +691,16 @@ bool Localize(cv::Mat const& test_descriptors, indices.push_back(cid); } + // If detector is SURF and parameters have not been set from commandline, + // then set them to SURF defaults. + // This is different from BRISK defaults. + if (detector_name == "SURF") { + if (gflags::GetCommandLineFlagInfoOrDie("early_break_landmarks").is_default) + early_break_landmarks = 100000; + if (gflags::GetCommandLineFlagInfoOrDie("num_min_localization_inliers").is_default) + google::SetCommandLineOption("num_min_localization_inliers", "100"); + } + // To turn on verbose localization for debugging // google::SetCommandLineOption("verbose_localization", "true"); @@ -717,8 +732,11 @@ bool Localize(cv::Mat const& test_descriptors, << all_matches[i].size() << " " << similarity_rank[i] << "\n"; total += similarity_rank[i]; - if (total >= early_break_landmarks) + if (total >= early_break_landmarks) { + if (FLAGS_verbose_localization) + std::cout << "total " << total << " early break landmarks " << early_break_landmarks << std::endl; break; + } } std::vector observations; diff --git a/localization/sparse_mapping/src/vocab_tree.cc b/localization/sparse_mapping/src/vocab_tree.cc index db6bafacc3..ca5518b65e 100644 --- a/localization/sparse_mapping/src/vocab_tree.cc +++ b/localization/sparse_mapping/src/vocab_tree.cc @@ -65,10 +65,100 @@ #pragma GCC diagnostic ignored "-Wdelete-non-virtual-dtor" #pragma GCC diagnostic push #include // BoW db that works with both float and binary descriptors +#include #pragma GCC diagnostic pop #include #include +#include + +using namespace std; // NOLINT (trying to modify this as little as possible) + +// stolen from file (this doesn't link for some reason?) +namespace DBoW2 { + +// -------------------------------------------------------------------------- + +void FSurf64::meanValue(const std::vector &descriptors, + FSurf64::TDescriptor &mean) { + mean.resize(0); + mean.resize(FSurf64::L, 0); + + float s = descriptors.size(); + + vector::const_iterator it; + for (it = descriptors.begin(); it != descriptors.end(); ++it) { + const FSurf64::TDescriptor &desc = **it; + for (int i = 0; i < FSurf64::L; i += 4) { + mean[i ] += desc[i ] / s; + mean[i+1] += desc[i+1] / s; + mean[i+2] += desc[i+2] / s; + mean[i+3] += desc[i+3] / s; + } + } +} + +// -------------------------------------------------------------------------- + +double FSurf64::distance(const FSurf64::TDescriptor &a, const FSurf64::TDescriptor &b) { + double sqd = 0.; + for (int i = 0; i < FSurf64::L; i += 4) { + sqd += (a[i ] - b[i ])*(a[i ] - b[i ]); + sqd += (a[i+1] - b[i+1])*(a[i+1] - b[i+1]); + sqd += (a[i+2] - b[i+2])*(a[i+2] - b[i+2]); + sqd += (a[i+3] - b[i+3])*(a[i+3] - b[i+3]); + } + return sqd; +} + +// -------------------------------------------------------------------------- + +std::string FSurf64::toString(const FSurf64::TDescriptor &a) { + stringstream ss; + for (int i = 0; i < FSurf64::L; ++i) { + ss << a[i] << " "; + } + return ss.str(); +} + +// -------------------------------------------------------------------------- + +void FSurf64::fromString(FSurf64::TDescriptor &a, const std::string &s) { + a.resize(FSurf64::L); + + stringstream ss(s); + for (int i = 0; i < FSurf64::L; ++i) { + ss >> a[i]; + } +} + +// -------------------------------------------------------------------------- + +void FSurf64::toMat32F(const std::vector &descriptors, + cv::Mat &mat) { + if (descriptors.empty()) { + mat.release(); + return; + } + + const int N = descriptors.size(); + const int L = FSurf64::L; + + mat.create(N, L, CV_32F); + + for (int i = 0; i < N; ++i) { + const TDescriptor& desc = descriptors[i]; + float *p = mat.ptr(i); + for (int j = 0; j < L; ++j, ++p) { + *p = desc[j]; + } + } +} + +// -------------------------------------------------------------------------- + +} // namespace DBoW2 + namespace sparse_mapping { @@ -109,6 +199,46 @@ class BinaryDB : public BriefDatabase { BriefDatabase(voc, flag, val){} }; +typedef ProtobufVocabulary FloatVocabulary; +typedef ProtobufDatabase SurfDatabase; + +// Thin wrappers around DBoW2 databases, to avoid exposing the +// originals in the header file for compilation speed. +class FloatDB : public SurfDatabase { + public: + explicit FloatDB(google::protobuf::io::ZeroCopyInputStream* input) : SurfDatabase(input) {} + FloatDB(FloatVocabulary const& voc, bool flag, int val): + SurfDatabase(voc, flag, val){} +}; + +template +std::string toBytes(const TDescriptor &a) { + std::ostringstream out(std::stringstream::out | std::stringstream::binary); + out.write((const char*)a.desc, a.size); + return out.str(); +} + +template +void fromBytes(TDescriptor &a, const std::string &s) { + if ((unsigned int)a.size != s.size()) + a.Initialize(s.size()); + memcpy(a.desc, s.c_str(), s.size()); +} + +// this is all a big hack +template<> +void fromBytes >(std::vector &a, const std::string &s) { + a.resize(s.size() / sizeof(float)); + memcpy(a.data(), s.c_str(), s.size()); +} + +template<> +std::string toBytes >(const std::vector &a) { + std::ostringstream out(std::stringstream::out | std::stringstream::binary); + out.write((const char*)a.data(), a.size() * sizeof(float)); + return out.str(); +} + template void ProtobufVocabulary::LoadProtobuf(google::protobuf::io::ZeroCopyInputStream* input) { // C++ is a dumb language, we have to put this in front of all member variables inherited from @@ -148,7 +278,7 @@ void ProtobufVocabulary::LoadProtobuf(google::protobuf::io::Zero this->m_nodes[nid].weight = weight; this->m_nodes[pid].children.push_back(nid); - F::fromBytes(this->m_nodes[nid].descriptor, d); + fromBytes(this->m_nodes[nid].descriptor, d); } // words @@ -227,7 +357,7 @@ void ProtobufVocabulary::SaveProtobuf(google::protobuf::io::Zero node.set_node_id(child.id); node.set_parent_id(pid); node.set_weight(child.weight); - node.set_feature(F::toBytes(child.descriptor)); + node.set_feature(toBytes(child.descriptor)); if (!WriteProtobufTo(node, output)) { LOG(FATAL) << "Failed to write db node to file."; } @@ -285,7 +415,7 @@ void ProtobufDatabase::SaveProtobuf(google::protobuf::io::ZeroCo // Constructor and destructor for VocabDB VocabDB::VocabDB(): - binary_db(NULL), m_num_nodes(0) { + binary_db(NULL), float_db(NULL), m_num_nodes(0) { } VocabDB::~VocabDB() { ResetDB(this); @@ -294,6 +424,8 @@ VocabDB::~VocabDB() { void VocabDB::SaveProtobuf(google::protobuf::io::ZeroCopyOutputStream* output) const { if (binary_db != NULL) { binary_db->SaveProtobuf(output); + } else if (float_db != NULL) { + float_db->SaveProtobuf(output); } else { LOG(ERROR) << "Unsupported database type."; } @@ -304,11 +436,18 @@ void VocabDB::LoadProtobuf(google::protobuf::io::ZeroCopyInputStream* input, int if (db_type == sparse_mapping_protobuf::Map::BINARYDB) { binary_db = new BinaryDB(input); m_num_nodes = binary_db->size(); + } else if (db_type == sparse_mapping_protobuf::Map::FLOATDB) { + float_db = new FloatDB(input); + m_num_nodes = float_db->size(); } else { LOG(ERROR) << "Using unsupported database type."; } } +int countPlaceValues(int number) { + return number == 0 ? 1 : std::floor(std::log10(std::abs(number))) + 1; +} + void BuildDB(std::string const& map_file, std::string const& descriptor, int depth, int branching_factor, int restarts) { @@ -320,7 +459,9 @@ void BuildDB(std::string const& map_file, int total_features = 0; for (size_t cid = 0; cid < map.GetNumFrames(); cid++) total_features += map.GetFrameKeypoints(cid).outerSize(); - while (pow(branching_factor, depth) < total_features) { + + // increase num words to be the same order of magnitude as num features + while (countPlaceValues(pow(branching_factor, depth)) < countPlaceValues(total_features)) { depth++; LOG(WARNING) << "Database not large enough, increasing depth."; } @@ -337,6 +478,10 @@ void ResetDB(VocabDB* db) { delete db->binary_db; db->binary_db = NULL; } + if (db->float_db != NULL) { + delete db->float_db; + db->float_db = NULL; + } } // These are defined here, rather than in the header file, @@ -356,7 +501,7 @@ void MatDescrToVec(cv::Mat const& mat, std::vector * vec) { (*vec).reserve(mat.cols); (*vec).clear(); for (int c = 0; c < mat.cols; c++) { - float val = static_cast(mat.at(0, c)); + float val = mat.at(0, c); (*vec).push_back(val); } } @@ -382,6 +527,9 @@ void QueryDB(std::string const& descriptor, VocabDB * vocab_db, std::vector * indices) { indices->clear(); + if (vocab_db->binary_db == NULL && vocab_db->float_db == NULL) + return; + if (vocab_db->binary_db != NULL) { assert(IsBinaryDescriptor(descriptor)); BinaryDB & db = *(vocab_db->binary_db); // shorten @@ -400,8 +548,22 @@ void QueryDB(std::string const& descriptor, VocabDB * vocab_db, indices->push_back(ret[j].Id); } } else { - // no database specified - return; + assert(!IsBinaryDescriptor(descriptor)); + FloatDB & db = *(vocab_db->float_db); // shorten + + std::vector descriptors_vec; + for (int r = 0; r < descriptors.rows; r++) { + DBoW2::FSurf64::TDescriptor descriptor; + MatDescrToVec(descriptors.row(r), &descriptor); + descriptors_vec.push_back(descriptor); + } + + DBoW2::QueryResults ret; + db.query(descriptors_vec, ret, num_similar); + + for (size_t j = 0; j < ret.size(); j++) { + indices->push_back(ret[j].Id); + } } return; @@ -411,13 +573,37 @@ void BuildDBforDBoW2(SparseMap* map, std::string const& descriptor, int depth, int branching_factor, int restarts) { int num_frames = map->GetNumFrames(); - const DBoW2::WeightingType weight = DBoW2::TF_IDF; const DBoW2::ScoringType score = DBoW2::L1_NORM; int num_features = 0; if (!IsBinaryDescriptor(descriptor)) { - LOG(ERROR) << "Using unsupported vocabulary database type."; + std::vector > features; + for (int cid = 0; cid < num_frames; cid++) { + int num_keys = map->GetFrameKeypoints(cid).outerSize(); + num_features += num_keys; + std::vector descriptors; + for (int i = 0; i < num_keys; i++) { + cv::Mat row = map->GetDescriptor(cid, i); + DBoW2::FSurf64::TDescriptor descriptor; + MatDescrToVec(row, &descriptor); + descriptors.push_back(descriptor); + } + features.push_back(descriptors); + } + FloatVocabulary voc(branching_factor, depth, weight, score); + if (countPlaceValues(num_features) > 7) { + LOG(WARNING) << "Using " << num_features << " features to build vocabulary. " + << "This may be too many for the vocab to build.\n"; + } + voc.create(features); + + FloatDB* db = new FloatDB(voc, false, 0); + for (size_t i = 0; i < features.size(); i++) + db->add(features[i]); + + map->vocab_db_.float_db = db; + map->vocab_db_.m_num_nodes = db->size(); } else { // Binary descriptors. For each image, copy them from a CV matrix // to a vector of vectors. Also extract individual bits from diff --git a/scripts/postprocessing/coverage_analysis/activity_db_generator.py b/scripts/postprocessing/coverage_analysis/activity_db_generator.py index d6b7f4d15f..c79df98a67 100644 --- a/scripts/postprocessing/coverage_analysis/activity_db_generator.py +++ b/scripts/postprocessing/coverage_analysis/activity_db_generator.py @@ -7,6 +7,7 @@ import sys import numpy as np +import rosbag import rospy from ff_msgs.msg import VisualLandmarks from tf.transformations import * @@ -103,6 +104,17 @@ def listener(self): rospy.Subscriber("/loc/ml/features", VisualLandmarks, self.callback) rospy.spin() + def read_from_bag(self, bag_name, file): + print("Reading from bag...") + + bag = rosbag.Bag(bag_name) + + for topic, msg, t in bag.read_messages(topics=["/loc/ml/features"]): + self.callback(msg) + + self.close_file() + print("...Done!") + if __name__ == "__main__": @@ -110,14 +122,35 @@ def listener(self): if len(sys.argv) < 5: print( - "Usage: activity_db_generator.py " + "Usage: activity_db_generator.py " ) print(" = 'YYYYMMDD'") print(" = 'mapName'") print(" = 'activityName'") print(" = '/location/to/save/'") + print(" *optional* = '/location/bag'") sys.exit(1) + elif len(sys.argv) == 6: + activity_date = sys.argv[1] + map_name = sys.argv[2] + activity_name = sys.argv[3] + location_name = sys.argv[4] + output_filename = ( + location_name + + activity_date + + "_" + + map_name + + "_" + + activity_name + + "_db.csv" + ) + bag_name = sys.argv[5] + + obj = Activity_DBGenerator() + obj.open_file(output_filename, activity_name, map_name, activity_date) + obj.read_from_bag(bag_name, output_filename) + else: activity_date = sys.argv[1] map_name = sys.argv[2] @@ -133,9 +166,9 @@ def listener(self): + "_db.csv" ) - obj = Activity_DBGenerator() - obj.open_file(output_filename, activity_name, map_name, activity_date) - obj.listener() + obj = Activity_DBGenerator() + obj.open_file(output_filename, activity_name, map_name, activity_date) + obj.listener() except KeyboardInterrupt: print("\n <-CTRL-C EXIT: USER manually exited!->") diff --git a/scripts/postprocessing/coverage_analysis/img_to_bag.py b/scripts/postprocessing/coverage_analysis/img_to_bag.py index 17a741f925..728c9f7d13 100755 --- a/scripts/postprocessing/coverage_analysis/img_to_bag.py +++ b/scripts/postprocessing/coverage_analysis/img_to_bag.py @@ -42,8 +42,10 @@ def create_mono_bag(imgs, bagname): bag = rosbag.Bag(bagname, "w") try: + print("Adding images to bag %s ..." % bagname) + for i in range(len(imgs)): - print("Adding %s" % imgs[i]) + # print("Adding %s" % imgs[i]) img = cv2.imread(imgs[i]) bridge = CvBridge() @@ -54,9 +56,10 @@ def create_mono_bag(imgs, bagname): img_msg.header.stamp = Stamp img_msg.header.frame_id = "camera" - bag.write("mgt/img_sampler/nav_cam/image_record", img_msg, Stamp) + bag.write("/mgt/img_sampler/nav_cam/image_record", img_msg, Stamp) finally: bag.close() + print("...Done!") def create_bag(args): diff --git a/scripts/postprocessing/coverage_analysis/readme.md b/scripts/postprocessing/coverage_analysis/readme.md new file mode 100755 index 0000000000..0bdae41d44 --- /dev/null +++ b/scripts/postprocessing/coverage_analysis/readme.md @@ -0,0 +1,134 @@ +# **COVERAGE ANALYSIS README** + +### Tools to generate a heatmap visualization in RViz (Gazebo) of the features distribution (or map coverage) for a given map or trajectory contained in a bag. If this process is used to observe the coverage in a given bag at a given activity, Step 1 can be skipped. + +## Steps: + +## 1. Generate a bag from all the images in the map of interest. +- Inputs: dir/where/jpg/are +- Tool: ~/coverage\_analysis/img\_to\_bag.py +- Outputs: bagOfTheMap.bag + +Move all the image files used to generate the map to a single directory. +Run +`python img_to_bag.py /dir/where/jpgs/are/dir/where/bag/should/go/bagOfTheMap.bag` +where +bagOfTheMap.bag = 20210518_cabanel.bag +If this gives an error, run rosdep update. + +## 2. Find ML landmarks +- Inputs: bagOfInterest.bag, mapToTestAgainst.map +- Tools: run_map_matcher.cc +- Outputs: output.bag + +In a terminal run +`rosrun localization_analysis run_map_matcher --bagfile --map-file -o /dir/where/to/save/output.bag -c ` + +## 3. Generate pose/ML features database. +- Inputs: activity\_name, activity\_date, bagOfInterest.bag +- Tools: ~/coverage\_analysis/activity\_db\_generator.py +- Outputs: activity\_database\_file with recommended naming convention "YYYYMMDD\_map\_activity\_db.csv" (The coordinates in the CSV output file +are in the Astrobee's body inertial frame already). + +Run in a terminal +`python ~/coverage_analysis/activity_db_generator.py ` +where +activity date = 20210726 +map name = dali +activity name = KiboEventRehRun1 +location to save = /dir/to/database/file/db.csv +The final file will be called /dir/to/database/file/20210726_dali_KiboEventRehRun1_db.csv + +## 4. Generate a coverage database +- Inputs: activity\_database\_file +- Tools: ~/coverage\_analysis/coverage_analyzer.py +- Outputs: activity\_coverage\_database_file. +- Temporary files: output\_features\_database.csv, output\_features\_database\_nonrepeat.csv + +This step first extracts from activity\_database.csv only the ML feature poses into a temporary file called, for example, 20210518\_cabanel\_features_db.csv or 20210726\_dali\_KiboEvRhRun1\_features_db.csv. + +It then identifies repeated ML features and copies only one of the repeated ML features into a temporary file called, for example, 20210518\_cabanel\_features\_db\_nonrepeat.csv or 20210726\_dali\_KiboEvRhRun1\_features\_db\_nonrepeat.csv These repeated features are unique, however it should be kept in mind that multiple cameras may be able to see the same feature. For visualization purposes is faster to have unique features. + +The next step generates the list of cube centers of size defined by grid\_size along the different walls in the JEM (Overhead, Aft, Forward, Deck). These walls are divided into the 8 bays of the JEM, where 1 corresponds to the bay closest to the entry node and 8 to the bay closest to the airlock. + +If a feature is within the volume of the cube centered at the pose being searched, a counter is increased and the final number of matches and the center of the cube is saved in the activity\_coverage\_database.csv file called, for example, 20210518\_cabanel\_coverage\_db.csv or 20210726\_dali\_KiboEvRhRun1\_coverage\_db.csv. + +The format of this file consist of Number-of-ML-Features-found-within-the-cube-defined-by X-Y-Z-Pose-of-the-center-of-the-cube-checked. + +Run in a terminal +`python ~/coverage_analysis/coverage_analyzer.py ` +where +activity\_database\_file = /dir/where/database/file/is/20210726\_dali\_KiboEvRhRun1\_db.csv, +database\_fileOut = /dir/where/results/are/going/to/be/placed/20210726\_dali\_KiboEvRhRun1\_features\_db.csv, +feat\_only\_fileOut = /dir/where/results/are/going/to/be/placed/20210726\_dali\_KiboEvRhRun1\_features\_db\_nonrepeat.csv, +activity\_coverage\_database\_file = /dir/where/results/are/going/to/be/placed/20210518\_cabanel\_coverage\_db.csv +or +activity\_coverage\_database\_file = /dir/where/results/are/going/to/be/placed/20210726\_dali\_KiboEvRhRun1\_db.csv + +At this point, the user has a few options: +- **1.** Generate a 3D heatmap visualization in Gazebo of the studied map's general coverage of the JEM +This is accomplished following Step 4. +- **2.** Generate a 3D animation and visualization in Gazebo of the studied robot's trajectory coverage at each pose of the trajectory +This is accomplished following Step 5. +- **3.** Generate a statistics report in PDF of the general and detailed coverage of the studied map or robot's trajectory's coverage +This is accomplished following Step 6. + +## 5. Generate Walls Heatmap visualization in Gazebo +- Inputs: activity\_coverage\_database\_file +- Tools: ~/coverage_painter.py, roslaunch +- Outputs: 30cm^3 grid painted as shown in rviz (Gazebo) + +In a terminal, run +`roslaunch astrobee sim.launch dds:=false speed:=0.75 rviz:=true` +to bring RViz up. If the Rviz display looks too cluttered, most topics can be turned off except "Registration". In another terminal run +`python ~/coverage_anaylisis/coverage_painter.py map_coverage` +where +activity\_coverage\_database\_file = /dir/where/results/are/going/to/be/placed/20210518\_cabanel\_coverage\_db.csv + +This publishes cubes representing the map coverage in 20210518_cabanel_coverage_db.csv according to the following color scheme: +- red if 0 registered ML features found in any cube +- orange if 1-10 registered ML features found in any cube +- yellow if 11-20 registered ML features found in any cube +- green if 21-40 registered ML features found in any cube +- blue if 40+ registered ML features found in any cube + +## 6. Generate Robot's Trajectory Heatmap visualization in RViz +- Inputs: activity\_database\_file +- Tools: ~/coverage\_painter.py, roslaunch +- Outputs: Animated trajectory consisting of each of the trajectory poses colored coded according to their coverage level + +In a terminal, run +`roslaunch astrobee sim.launch dds:=false speed:=0.75 rviz:=true` +to bring RViz up. If the Rviz display looks too cluttered, most topics can be turned off except "Registration". In another terminal run +`python ~/coverage/anaylisis/coverage_painter.py robot_coverage` +where +activity\_database\_file = /dir/where/database/file/is/20210726\_dali\_KiboEvRhRun1\_db.csv + +This will publish each pose contained in the trajectory and will color code them according to the following color scheme: +- red if 0 registered ML features found in any cube +- orange if 1-10 registered ML features found in any cube +- yellow if 11-20 registered ML features found in any cube +- green if 21-40 registered ML features found in any cube +- blue if 40+ registered ML features found in any cube + +## 7. Generate stats report +- Inputs: activity\_coverage\_database\_file = /dir/where/map-coverage/file/is/20210518\_cabanel\_coverage\_db.csv or /dir/where/robot-trajectory-coverage/file/is/20210726\_dali\_KiboEvRhRun1\_coverage\_db.csv +- Tools: ~/coverage\_stats\_reporter.py +- Outputs: CSV and PDF Stats reports, saved at the input file's same location +1) CSV: /dir/where/map-coverage/file/is/20210707\_dali\_coverage\_db\_stats.csv or /dir/where/robot-trajector-coverage/file/is/20210726\_dali\_KiboEvRhRun1\_coverage\_db\_stats.csv +2) PDF: /dir/where/map-coverage/file/is/20210707\_dali\_coverage\_db\_stats\_report.pdf or /dir/where/robot-trajector-coverage/file/is/20210726\_dali\_KiboEvRhRun1\_coverage\_db\_stats\_report.pdf + +Generates a statistics report in CSV and PDF of the coverage on each wall (Overhead, Aft, Forward, Deck) and airlock based on the number of registered ML features found on every cube analyzed. +The report consists of two images: +- the general map or bag coverage distribution and the +- the detailed map or bag coverage distribution. + +It provides the coverage percentage based on the number of registered ML features at each wall distributed across five ranges: 0ML, 1-10ML, 11-20ML, 21-40ML, and 40+ML, as well as the total number of registered ML features in the analized wall, and the percentage of the total number of ML features analyzed within the JEM walls and airlock. +In a terminal run +`python ~/coverage_analysis/coverage_stats_reporter.py ` +where +activity\_coverage\_database\_file = /dir/where/map-coverage/file/is/20210518\_cabanel\_coverage\_db.csv +or +activity\_coverage\_database\_file = /dir/where/robot-trajectory-coverage/file/is/20210726\_dali\_KiboEvRhRun1\_coverage\_db.csv + + diff --git a/scripts/postprocessing/coverage_analysis/readme.txt b/scripts/postprocessing/coverage_analysis/readme.txt deleted file mode 100644 index 1c9908ba1b..0000000000 --- a/scripts/postprocessing/coverage_analysis/readme.txt +++ /dev/null @@ -1,128 +0,0 @@ -Tools to generate a heatmap visualization in RViz (Gazebo) of the features distribution (or map coverage) for a given map or trajectory contained in a bag. -If this process is used to observe the coverage in a given bag at a given activity, Step 1 can be skipped. - -Steps: -1. Generate a bag from all the images in the map of interest. -Inputs: dir/where/jpg are -Tool: ~/coverage_analysis/img_to_bag.py -Outputs: bagOfTheMap.bag - -Move all the image files used to generate the map to a single directory -Run "python img_to_bag.py /dir/where/jpgs/are /dir/where/bag/should/go/bagOfTheMap.bag", bagOfTheMap.bag = 20210518_cabanel.bag -If this gives an error, run rosdep update. - -2. Run the localization node to find ML landmarks -Inputs: activity_name, activity_date, bagOfInterest.bag -Tools: roslaunch, rosservice, ~/coverage_analysis/activity_db_generator.py, rosbag play -Outputs: activity_database_file with recommended naming convention "YYYYMMDD_map_activity_db.csv" (The coordinates in the CSV output file are in the Astrobee's body inertial frame already). - -Follow the instructions in https://github.com/nasa/astrobee/blob/master/localization/sparse_mapping/build_map.md for "Verify localization against -a sparse map on a local machine" - -Make sure your environment variables are correct, and that the symbolic link in ~/astrobee/astrobee/resources/maps/iss.map points to the map of interest. -To do this, first check iss.map is pointing to your map of interest by running in a terminal "ls -lah~/astrobee/astrobee/resources/maps/iss.map". -If it is not pointing to the map of interest, move the current map, and make it point to the correct one by running in a terminal - "mv ~/astrobee/astrobee/resources/maps/iss.map ~/astrobee/astrobee/resources/maps/iss_backup.map" - "ln -s ~/map/location/.map ~/astrobee/astrobee/resources/maps/iss.map" -Run in a terminal - "roslaunch astrobee astrobee.launch mlp:=local llp:=disabled nodes:=framestore,localization_node world:=iss". -In another terminal enable localization: - "rosservice call /loc/ml/enable true" and wait for it to return "success: True". -In this same terminal, run -"python ~/coverage_analysis/activity_db_generator.py ", where -activity date = 20210726 -map name = dali -activity name = KiboEventRehRun1 -location to save = /dir/where/database/file/will/be/ -The final file will be /dir/where/database/file/will/be/20210726_dali_KiboEventRehRun1_db.csv - -After starting activity_DatabaseGenerator.py, run in another terminal the bag just created: -"rosbag play bagOfInterest.bag /mgt/img_sampler/nav_cam/image_record:=/hw/cam_nav" -If necessary, once the database of ML features and robot poses is generated change back the map as it was by running in a terminal -"rm ~/astrobee/astrobee/resources/maps/iss.map; mv ~/astrobee/astrobee/resources/maps/iss_backup.map ~/astrobee/astrobee/resources/maps/iss.map" - -After the process has finished, Ctrl-C all the terminals. - -3. Generate a coverage database -Inputs: activity_database_file -Tools: ~/coverage_analysis/coverage_analyzer.py -Outputs: activity_coverage_database_file. Temporary files: output_features_database.csv, output_features_database_nonrepeat.csv, - -This step first extracts from activity_database.csv only the ML feature poses into a temporary file called, for example, 20210518_cabanel_features_db.csv or 20210726_dali_KiboEvRhRun1_features_db.csv. -It then identifies repeated ML features and copies only one of the repeated ML features into a temporary file called, for example, 20210518_cabanel_features_db_nonrepeat.csv or 20210726_dali_KiboEvRhRun1_features_db_nonrepeat.csv -These repeated features are unique, however it should be kept in mind that multiple cameras may be able to see the same feature. For visualization purposes is faster to have unique features. -The next step generates the list of cube centers of size defined by grid_size along the different walls in the JEM (Overhead, Aft, Forward, Deck). -These walls are divided into the 8 bays of the JEM, where 1 corresponds to the bay closest to the entry node and 8 to the bay closest to the airlock. -If a feature is within the volume of the cube centered at the pose being searched, a counter is increased and the final number of matches and the center of the cube is saved in the activity_coverage_database.csv file called, for example, -20210518_cabanel_coverage_db.csv or 20210726_dali_KiboEvRhRun1_coverage_db.csv -The format of this file consist of Number-of-ML-Features-found-within-the-cube-defined-by X-Y-Z-Pose-of-the-center-of-the-cube-checked. - -Run -"python ~/coverage_analysis/coverage_analyzer.py " where -activity_database_file = /dir/where/database/file/is/20210726_dali_KiboEvRhRun1_db.csv, -database_fileOut = /dir/where/results/are/going/to/be/placed/20210726_dali_KiboEvRhRun1_features_db.csv, -feat_only_fileOut = /dir/where/results/are/going/to/be/placed/20210726_dali_KiboEvRhRun1_features_db_nonrepeat.csv, and -activity_coverage_database_file = /dir/where/results/are/going/to/be/placed/20210518_cabanel_coverage_db.csv or /dir/where/results/are/going/to/be/placed/20210726_dali_KiboEvRhRun1_db.csv - -At this point, the user has a few options: -1. Generate a 3D heatmap visualization in Gazebo of the studied map's general coverage of the JEM - This is accomplished following Step 4. -2. Generate a 3D animation and visualization in Gazebo of the studied robot's trajectory coverage at each pose of the trajectory - This is accomplished following Step 5. -3. Generate a statistics report in PDF of the general and detailed coverage of the studied map or robot's trajectory's coverage - This is accomplished following Step 6. - -4. Generate Walls Heatmap visualization in Gazebo -Inputs: activity_coverage_database_file -Tools: ~/coverage_painter.py, roslaunch -Outputs: 30cm^3 grid painted as shown in rviz (Gazebo) - -In a terminal, run - "roslaunch astrobee sim.launch dds:=false speed:=0.75 rviz:=true" to bring RViz up. -If the Rviz display looks too cluttered, most topics can be turned off except "Registration". -In another terminal run - "python ~/coverage_anaylisis/coverage_painter.py map_coverage" where - -activity_coverage_database_file = /dir/where/results/are/going/to/be/placed/20210518_cabanel_coverage_db.csv - -This publishes cubes representing the map coverage in 20210518_cabanel_coverage_db.csv according to the following color scheme: -red if 0 registered ML features found in any cube are present in this cube -orange if 1-10 registered ML features found in any cube are present in this cube -yellow if 11-20 registered ML features found in any cube are present in this cube -green if 21-40 registered ML features found in any cube are present in this cube -blue if 40+ registered ML features found in any cube are present in this cube - -5. Generate Robot's Trajectory Heatmap visualization in RViz -Inputs: activity_database_file -Tools: ~/coverage_painter.py, roslaunch -Outputs: Animated trajectory consisting of each of the trajectory poses colored coded according to their coverage level - -In a terminal, run "roslaunch astrobee sim.launch dds:=false speed:=0.75 rviz:=true" to bring RViz up. -If the Rviz display looks too cluttered, most topics can be turned off except "Registration". -In another terminal run - "python ~/coverage/anaylisis/coverage_painter.py robot_coverage" where - -activity_database_file = /dir/where/database/file/is/20210726_dali_KiboEvRhRun1_db.csv, - -This will publish each pose contained in the trajectory and will color code them according to the following color scheme: -red if 0 registered ML features found in any cube are present in this cube -orange if 1-10 registered ML features found in any cube are present in this cube -yellow if 11-20 registered ML features found in any cube are present in this cube -green if 21-40 registered ML features found in any cube are present in this cube -blue if 40+ registered ML features found in any cube are present in this cube - -6. Generate stats report -Inputs: activity_coverage_database_file = /dir/where/map-coverage/file/is/20210518_cabanel_coverage_db.csv or /dir/where/robot-trajectory-coverage/file/is/20210726_dali_KiboEvRhRun1_coverage_db.csv -Tools: ~/coverage_stats_reporter.py -Outputs: CSV and PDF Stats reports, saved at the input file's same location -(CSV: /dir/where/map-coverage/file/is/20210707_dali_coverage_db_stats.csv or /dir/where/robot-trajector-coverage/file/is/20210726_dali_KiboEvRhRun1_coverage_db_stats.csv) -(PDF: /dir/where/map-coverage/file/is/20210707_dali_coverage_db_stats_report.pdf or /dir/where/robot-trajector-coverage/file/is/20210726_dali_KiboEvRhRun1_coverage_db_stats_report.pdf) - -Generates a statistics report in CSV and PDF of the coverage on each wall (Overhead, Aft, Forward, Deck) and airlock based on the number of registered ML features found on every cube analyzed. -The report consists of two images: 1) the general map or bag coverage distribution and the 2) the detailed map or bag coverage distribution. -It provides the coverage percentage based on the number of registered ML features at each wall distributed across five ranges: 0ML, 1-10ML, 11-20ML, 21-40ML, and 40+ML, as well as the total number of registered ML features in -the analized wall, and the percentage of the total number of ML features analyzed within the JEM walls and airlock. -In a terminal run - "python ~/coverage_analysis/coverage_stats_reporter.py " where -activity_coverage_database_file = /dir/where/map-coverage/file/is/20210518_cabanel_coverage_db.csv or /dir/where/robot-trajectory-coverage/file/is/20210726_dali_KiboEvRhRun1_coverage_db.csv - diff --git a/scripts/setup/install_desktop_packages.sh b/scripts/setup/install_desktop_packages.sh index 163e42f148..2ecae00978 100755 --- a/scripts/setup/install_desktop_packages.sh +++ b/scripts/setup/install_desktop_packages.sh @@ -46,7 +46,7 @@ then # Add these packages to the apt sources (we remove them later, so apt update succeeds) - NO_TUNNEL=${NO_TUNNEL:-0} # Override this with 1 before invoking if the tunnel is not desired + NO_TUNNEL=1 # Override this with 1 before invoking if the tunnel is not desired if [ "${NO_TUNNEL}" -eq 1 ]; then echo "Getting the custom Debian without tunnel" diff --git a/submodules/CATKIN_IGNORE b/submodules/CATKIN_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/tools/localization_analysis/CMakeLists.txt b/tools/localization_analysis/CMakeLists.txt index a0ceac75e2..653c8ec448 100644 --- a/tools/localization_analysis/CMakeLists.txt +++ b/tools/localization_analysis/CMakeLists.txt @@ -66,6 +66,7 @@ if (USE_ROS) src/graph_localizer_simulator.cc src/imu_bias_tester_adder.cc src/live_measurement_simulator.cc + src/map_matcher.cc src/parameter_reader.cc src/sparse_mapping_pose_adder.cc src/utilities.cc @@ -103,6 +104,12 @@ if (USE_ROS) target_link_libraries(run_imu_bias_tester_adder ${PROJECT_NAME} gflags gtsam ${catkin_LIBRARIES}) + ## Declare a C++ executable: run_map_matcher + add_executable(run_map_matcher tools/run_map_matcher.cc) + add_dependencies(run_map_matcher ${catkin_EXPORTED_TARGETS}) + target_link_libraries(run_map_matcher + ${PROJECT_NAME} gflags gtsam ${catkin_LIBRARIES}) + ## Declare a C++ executable: run_sparse_mapping_pose_adder add_executable(run_sparse_mapping_pose_adder tools/run_sparse_mapping_pose_adder.cc) add_dependencies(run_sparse_mapping_pose_adder ${catkin_EXPORTED_TARGETS}) @@ -125,6 +132,7 @@ install(TARGETS run_bag_imu_filterer DESTINATION bin) install(TARGETS run_depth_odometry_adder DESTINATION bin) install(TARGETS run_graph_bag DESTINATION bin) install(TARGETS run_imu_bias_tester_adder DESTINATION bin) +install(TARGETS run_map_matcher DESTINATION bin) install(TARGETS run_sparse_mapping_pose_adder DESTINATION bin) install(CODE "execute_process( COMMAND ln -s ../../bin/convert_depth_msg share/${PROJECT_NAME} @@ -132,6 +140,7 @@ install(CODE "execute_process( COMMAND ln -s ../../bin/run_depth_odometry_adder share/${PROJECT_NAME} COMMAND ln -s ../../bin/run_graph_bag share/${PROJECT_NAME} COMMAND ln -s ../../bin/run_imu_bias_tester_adder share/${PROJECT_NAME} + COMMAND ln -s ../../bin/run_map_matcher share/${PROJECT_NAME} COMMAND ln -s ../../bin/run_sparse_mapping_pose_adder share/${PROJECT_NAME} WORKING_DIRECTORY ${CMAKE_INSTALL_PREFIX} OUTPUT_QUIET diff --git a/tools/localization_analysis/include/localization_analysis/map_matcher.h b/tools/localization_analysis/include/localization_analysis/map_matcher.h new file mode 100644 index 0000000000..020bb2f2d4 --- /dev/null +++ b/tools/localization_analysis/include/localization_analysis/map_matcher.h @@ -0,0 +1,60 @@ +/* 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 LOCALIZATION_ANALYSIS_MAP_MATCHER_H_ +#define LOCALIZATION_ANALYSIS_MAP_MATCHER_H_ + +#include +#include +#include +#include + +#include + +#include + +#include + +namespace localization_analysis { +class MapMatcher { + public: + MapMatcher(const std::string& input_bag_name, const std::string& map_file, const std::string& image_topic, + const std::string& output_bag_name, const std::string& config_prefix = "", + const std::string& save_noloc_imgs = ""); + void AddMapMatches(); + void LogResults(); + + private: + bool GenerateVLFeatures(const sensor_msgs::ImageConstPtr& image_msg, ff_msgs::VisualLandmarks& vl_features); + + rosbag::Bag input_bag_; + rosbag::Bag output_bag_; + rosbag::Bag nonloc_bag_; + std::string image_topic_; + sparse_mapping::SparseMap map_; + localization_node::Localizer map_feature_matcher_; + std::string config_prefix_; + gtsam::Pose3 body_T_nav_cam_; + localization_common::Averager feature_averager_; + int sparse_mapping_min_num_landmarks_; + int match_count_; + int image_count_; +}; +} // namespace localization_analysis + +#endif // LOCALIZATION_ANALYSIS_MAP_MATCHER_H_ diff --git a/tools/localization_analysis/results.csv b/tools/localization_analysis/results.csv new file mode 100644 index 0000000000..a9cc8efcf9 --- /dev/null +++ b/tools/localization_analysis/results.csv @@ -0,0 +1,12 @@ +rmse,0.02223592997076043 +orientation_rmse,0.014439809825783571 +integrated_rmse,0.036124483626381716 +rel_rmse,0.02892175714834411 +rel_orientation_rmse,0.01689468156099405 +rel_integrated_rmse,0.023760211093031655 +imu_augmented_rmse,0.01909813576341352 +imu_augmented_orientation_rmse,0.016061346181761867 +imu_augmented_integrated_rmse,0.07223932449464295 +rel_imu_augmented_rmse,0.029713320157916855 +rel_imu_augmented_orientation_rmse,0.01803531904369424 +rel_imu_augmented_integrated_rmse,0.025856844417313004 diff --git a/tools/localization_analysis/scripts/analyze_gaps.py b/tools/localization_analysis/scripts/analyze_gaps.py new file mode 100644 index 0000000000..1ce91a9e61 --- /dev/null +++ b/tools/localization_analysis/scripts/analyze_gaps.py @@ -0,0 +1,222 @@ +import rosbag +import rospy +import argparse +import os +import glob +import numpy as np +import matplotlib +import matplotlib.pyplot as plt +import math +import rospy +import tf +from geometry_msgs.msg import Quaternion + +matplotlib.use("pdf") +from matplotlib.backends.backend_pdf import PdfPages + +def quat_to_rpy(quaternion): + # Convert Quaternion to RPY + quaternion_tuple = (quaternion.x, quaternion.y, quaternion.z, quaternion.w) + rpy = tf.transformations.euler_from_quaternion(quaternion_tuple) + return rpy + +def find_bag(gt_bag, bag_dir): + for root, dirs, files in os.walk(bag_dir): + for file_name in files: + search_for = gt_bag + if file_name == search_for: + file_path = os.path.join(root, file_name) + return file_path + return "" + + +def plot_histogram(data, name, ax, log_scale = False, bucket_size = 5, ylim = (0, 30), xlim = (5, 280), color = 'blue'): + ax.hist(data, bins=np.arange(min(data), max(data) + bucket_size, bucket_size), + edgecolor='black', alpha = 0.5, label = name, color = color) # 'auto' selects the number of bins automatically + ax.set_xlabel('Gap sizes (s)') + ax.set_ylabel('Frequency') + ax.legend() + ax.set_xlim(xlim) + if log_scale: + ax.set_title('Gap size distribution (Log Scale)') + ax.set_yscale('log') + else: + ax.set_title('Gap size distribution') + ax.set_ylim(ylim) + +def calculate_rmse(bag1_file, bag2_file, topic): + # Open the bags + bag1 = rosbag.Bag(bag1_file) + bag2 = rosbag.Bag(bag2_file) + + # Get the timestamps from both bags + bag1_timestamps = {ts.to_nsec(): msg for _, msg, ts in bag1.read_messages(topics=[topic])} + bag2_timestamps = {ts.to_nsec(): msg for _, msg, ts in bag2.read_messages(topics=[topic])} + + # Find the common timestamps in both bags + common_timestamps = set(bag1_timestamps.keys()).intersection(bag2_timestamps.keys()) + if (len(common_timestamps)) < 20: + return np.empty((0, 3), dtype=float), np.empty((0, 3), dtype=float) + + # Extract x, y, z positions for messages with the same timestamp + positions1 = [] + positions2 = [] + + # Extract r, p, y positions for messages with the same timestamp + orientations1 = [] + orientations2 = [] + + for timestamp in common_timestamps: + positions1.append((bag1_timestamps[timestamp].pose.position.x, + bag1_timestamps[timestamp].pose.position.y, + bag1_timestamps[timestamp].pose.position.z)) + + positions2.append((bag2_timestamps[timestamp].pose.position.x, + bag2_timestamps[timestamp].pose.position.y, + bag2_timestamps[timestamp].pose.position.z)) + + rpy1 = quat_to_rpy(bag1_timestamps[timestamp].pose.orientation) + rpy2 = quat_to_rpy(bag2_timestamps[timestamp].pose.orientation) + orientations1.append(rpy1) + orientations2.append(rpy2) + + # Convert positions to numpy arrays + positions1 = np.array(positions1) + positions2 = np.array(positions2) + orientations1 = np.array(orientations1) + orientations2 = np.array(orientations2) + + # # Calculate the squared differences between positions + # squared_diff = (positions1 - positions2) ** 2 + + # # Calculate the RMSE for each position (x, y, z) + # rmse = np.sqrt(np.mean(squared_diff, axis=0)) + + # Close the bags + bag1.close() + bag2.close() + + return positions1, positions2, orientations1, orientations2 + +def find_message_gaps(bag_file, topic, min_gap, start_time, end_time): + bag = rosbag.Bag(bag_file) + + gap_list = [] + prev_time = start_time + + try: + for topic, msg, timestamp in bag.read_messages(topics=[topic]): + current_time = msg.header.stamp.to_sec() + + if prev_time is not None and current_time - prev_time > min_gap: + gap_list.append((prev_time, current_time)) + + prev_time = current_time + + # Check for gap at the end + if end_time - prev_time > min_gap: + gap_list.append((current_time, end_time)) + except Exception as e: + print(f"Error reading {bag_file}") + print(e) + bag.close() + return None + + bag.close() + return gap_list + +def find_all_gaps(directory, orig_dir, min_gap_size, verbose): + all_gaps_surf = [] + all_gaps_brisk = [] + brisk_positions_for_rmse = np.empty((0, 3), dtype=float) + surf_positions_for_rmse = np.empty((0, 3), dtype=float) + brisk_oris_for_rmse = np.empty((0, 3), dtype=float) + surf_oris_for_rmse = np.empty((0, 3), dtype=float) + total_duration = 0 + brisk_bag_files = glob.glob(os.path.join(directory, '*brisk.bag')) + for brisk_bag_file in brisk_bag_files: + bag_file = brisk_bag_file.replace("_brisk.bag", "_groundtruth.bag").split("/")[-1] + bag_file = find_bag(bag_file, orig_dir) + if verbose: + print(f"Processing {bag_file}") + try: + start_time = rosbag.Bag(bag_file).get_start_time() + end_time = rosbag.Bag(bag_file).get_end_time() + gaps_brisk = find_message_gaps(brisk_bag_file, '/sparse_mapping/pose', min_gap_size, start_time, end_time) + gaps_surf = find_message_gaps(brisk_bag_file.replace("brisk.bag", "surf.bag"), '/sparse_mapping/pose', min_gap_size, start_time, end_time) + if gaps_brisk is None or gaps_surf is None: + continue + brisk_poses, surf_poses, brisk_oris, surf_oris = (calculate_rmse(brisk_bag_file, brisk_bag_file.replace("brisk.bag", "surf.bag"), '/sparse_mapping/pose')) + # print(brisk_poses.shape, brisk_positions_for_rmse.shape) + brisk_positions_for_rmse = np.concatenate((brisk_positions_for_rmse, brisk_poses)) + surf_positions_for_rmse = np.concatenate((surf_positions_for_rmse, surf_poses)) + brisk_oris_for_rmse = np.concatenate((brisk_oris_for_rmse, brisk_oris)) + surf_oris_for_rmse = np.concatenate((surf_oris_for_rmse, surf_oris)) + total_duration += end_time - start_time + for gap in gaps_brisk: + all_gaps_brisk.append(gap[1] - gap[0]) + for gap in gaps_surf: + all_gaps_surf.append(gap[1] - gap[0]) + except Exception as e: + print(f"Error processing {bag_file}") + print(e) + # Create a new PDF file + with PdfPages('gap_analysis.pdf') as pdf: + fig, (ax1, ax2) = plt.subplots(1,2) + + plot_histogram(all_gaps_brisk, "BRISK", ax1) + plot_histogram(all_gaps_surf, "SURF", ax2, color = 'orange') + pdf.savefig() + + fig3, ax3 = plt.subplots() + plot_histogram(all_gaps_brisk, "BRISK", ax3) + plot_histogram(all_gaps_surf, "SURF", ax3, color = 'orange') + + # Save the figure to the PDF file + pdf.savefig() + + fig3, ax3 = plt.subplots() + plot_histogram(all_gaps_brisk, "BRISK (Log Scale)", ax3, log_scale=True) + plot_histogram(all_gaps_surf, "SURF (Log Scale)", ax3, log_scale=True, color = 'orange') + + # Save the figure to the PDF file + pdf.savefig() + + # Create a figure without axes + fig = plt.figure() + + surf_coverage = (total_duration - sum(all_gaps_surf)) / total_duration * 100 + brisk_coverage = (total_duration - sum(all_gaps_brisk)) / total_duration * 100 + + # Calculate the squared differences between positions + squared_diff = (brisk_positions_for_rmse - surf_positions_for_rmse) ** 2 + squared_diff_rpy = (brisk_oris_for_rmse - surf_oris_for_rmse) ** 2 + + # Calculate the RMSE for each position (x, y, z) + rmse = np.sqrt(np.mean(squared_diff, axis=0)) + rmse_rpy = np.sqrt(np.mean(squared_diff_rpy, axis=0)) + + # Add text directly to the figure + plt.text(0.5, 0.4, 'SURF Coverage: {}%'.format(surf_coverage), ha='center', va='center', fontsize=12) + plt.text(0.5, 0.5, 'BRISK Coverage: {}%'.format(brisk_coverage), ha='center', va='center', fontsize=12) + plt.text(0.5, 0.6, 'XYZ RMSE: {}'.format(rmse), ha='center', va='center', fontsize=12) + plt.text(0.5, 0.7, 'RPY RMSE: {}'.format(rmse_rpy), ha='center', va='center', fontsize=12) + + # Remove axes and ticks + plt.axis('off') + + # Add the axes to the PDF file + pdf.savefig() + + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter + ) + parser.add_argument("-d", "--matched_dir", help="Directory of matched bags.") + parser.add_argument("-g", "--orig_dir", help="Directory of original bags.") + parser.add_argument("-m", "--min_gap_size", help="Min size to count as a gap.") + parser.add_argument("-v", "--verbose", action="store_true", help="Print verbose output.") + args = parser.parse_args() + find_all_gaps(args.matched_dir, args.orig_dir, float(args.min_gap_size), args.verbose) + diff --git a/tools/localization_analysis/scripts/plot_detector_comparison.py b/tools/localization_analysis/scripts/plot_detector_comparison.py new file mode 100644 index 0000000000..5e738b3361 --- /dev/null +++ b/tools/localization_analysis/scripts/plot_detector_comparison.py @@ -0,0 +1,173 @@ +#!/usr/bin/python3 + +import argparse +import os +import sys + +import matplotlib + +import loc_states +import plot_helpers +import poses +import rmse_utilities +import utilities +import vector3d_plotter +import velocities + +matplotlib.use("pdf") +import csv +import math + +import geometry_msgs +import matplotlib.pyplot as plt +import rosbag +from plot_results import load_pose_msgs +from matplotlib.backends.backend_pdf import PdfPages + +def add_plots( + pdf, + surf_poses, + brisk_poses, + groundtruth_poses, +): + # colors = ["r", "b", "g"] + position_plotter = vector3d_plotter.Vector3dPlotter( + "Time (s)", "Position (m)", "Groundtruth vs. Sparse Mapping Position", True + ) + if brisk_poses: + position_plotter.add_pose_position( + brisk_poses, + linestyle="None", + colors = ["b", "b", "b"], + marker="o", + markeredgewidth=0.1, + markersize=1.5, + name="BRISK" + ) + position_plotter.add_pose_position( + surf_poses, + linestyle="None", + colors = ["r", "r", "r"], + marker="o", + markeredgewidth=0.1, + markersize=1.5, + name="SURF" + ) + + if groundtruth_poses: + position_plotter.add_pose_position( + groundtruth_poses, + linestyle="None", + colors = ["g", "g", "g"], + marker="o", + markeredgewidth=0.1, + markersize=1.5, + name="Groundtruth" + ) + position_plotter.plot(pdf) + + # orientations + orientation_plotter = vector3d_plotter.Vector3dPlotter( + "Time (s)", "Orientation (deg)", "Groundtruth vs. Sparse Mapping Orientation", True + ) + if brisk_poses: + orientation_plotter.add_pose_orientation( + brisk_poses, + linestyle="None", + marker="o", + colors = ["b", "b", "b"], + markeredgewidth=0.1, + markersize=1.5, + name="BRISK" + ) + orientation_plotter.add_pose_orientation( + surf_poses, + linestyle="None", + marker="o", + colors = ["r", "r", "r"], + markeredgewidth=0.1, + markersize=1.5, + name="SURF" + ) + if groundtruth_poses: + orientation_plotter.add_pose_orientation( + groundtruth_poses, + linestyle="None", + marker="o", + colors = ["g", "g", "g"], + markeredgewidth=0.1, + markersize=1.5, + name="Groundtruth" + ) + orientation_plotter.plot(pdf) + +# Groundtruth bag must have the same start time as other bagfile, otherwise RMSE calculations will be flawed +def create_plots( + surf_bagfile, + brisk_bagfile, + groundtruth_bagfile, + output_pdf_file, +): + surf_bag = rosbag.Bag(surf_bagfile) + brisk_bag = rosbag.Bag(brisk_bagfile) if brisk_bagfile else None + groundtruth_bag = rosbag.Bag(groundtruth_bagfile) if groundtruth_bagfile else None + bag_start_time = surf_bag.get_start_time() + + surf_poses = poses.Poses("Sparse Mapping", "/sparse_mapping/pose") + brisk_poses = poses.Poses("Sparse Mapping", "/sparse_mapping/pose") if brisk_bag else None + groundtruth_poses = poses.Poses("Sparse Mapping", "/sparse_mapping/pose") if groundtruth_bagfile else None + surf_vec_of_poses = [surf_poses] + brisk_vec_of_poses = [brisk_poses] if brisk_bag else None + groundtruth_vec_of_poses = [groundtruth_poses] if groundtruth_bagfile else None + load_pose_msgs(surf_vec_of_poses, surf_bag, bag_start_time) + load_pose_msgs(brisk_vec_of_poses, brisk_bag, bag_start_time) if brisk_bag else None + load_pose_msgs(groundtruth_vec_of_poses, groundtruth_bag, bag_start_time) if groundtruth_bagfile else None + + surf_bag.close() + if brisk_bag is not None: + brisk_bag.close() + if groundtruth_bag is not None: + groundtruth_bag.close() + + with PdfPages(output_pdf_file) as pdf: + add_plots( + pdf, + surf_poses, + brisk_poses, + groundtruth_poses, + ) + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter + ) + parser.add_argument("-s", "--surf-bagfile", help="Input surf bagfile.") + parser.add_argument( + "-b", + "--brisk-bagfile", + default=None, + help="Input brisk bagfile.", + ) + parser.add_argument( + "-g", + "--groundtruth-bagfile", + default=None, + help="bagfile containing groundtruth poses to use as a comparison for poses in the input bagfile. If none provided, sparse mapping poses are used as groundtruth from the input bagfile if available.", + ) + parser.add_argument("--output-file", default="output.pdf", help="Output pdf file.") + args = parser.parse_args() + if not os.path.isfile(args.surf_bagfile): + print(("Bag file " + args.surf_bagfile + " does not exist.")) + sys.exit() + if args.brisk_bagfile is not None and not os.path.isfile(args.brisk_bagfile): + print(("Bag file " + args.brisk_bagfile + " does not exist.")) + sys.exit() + if args.groundtruth_bagfile and not os.path.isfile(args.groundtruth_bagfile): + print(("Groundtruth Bag file " + args.groundtruth_bagfile + " does not exist.")) + sys.exit() + create_plots( + args.surf_bagfile, + args.brisk_bagfile, + args.groundtruth_bagfile, + args.output_file + ) diff --git a/tools/localization_analysis/scripts/plot_results.py b/tools/localization_analysis/scripts/plot_results.py index d8ff6fd13b..d01ef3932c 100755 --- a/tools/localization_analysis/scripts/plot_results.py +++ b/tools/localization_analysis/scripts/plot_results.py @@ -104,6 +104,9 @@ def add_graph_plots( orientation_plotter.add_pose_orientation(graph_localization_states) orientation_plotter.plot(pdf) + if len(graph_localization_states.times) == 0: + return + # Imu Augmented Loc vs. Loc position_plotter = vector3d_plotter.Vector3dPlotter( "Time (s)", "Position (m)", "Graph vs. IMU Augmented Graph Position", True @@ -867,9 +870,13 @@ def create_plots( sparse_mapping_poses, ar_tag_poses, ) - else: + elif len(graph_localization_states.times) > 0: add_other_loc_plots( - pdf, graph_localization_states, graph_localization_states + pdf, + graph_localization_states, + graph_localization_states, + sparse_mapping_poses, + ar_tag_poses, ) if has_depth_odom: depth_odom_poses = utilities.make_absolute_poses_from_relative_poses( diff --git a/tools/localization_analysis/scripts/results.csv b/tools/localization_analysis/scripts/results.csv new file mode 100644 index 0000000000..7ba9fd02c6 --- /dev/null +++ b/tools/localization_analysis/scripts/results.csv @@ -0,0 +1,120 @@ +rmse,0.0 +orientation_rmse,0.0 +integrated_rmse,0.0 +rel_rmse,0.0 +rel_orientation_rmse,0.0 +rel_integrated_rmse,0.0 +imu_augmented_rmse,0.0 +imu_augmented_orientation_rmse,0.0 +imu_augmented_integrated_rmse,0.0 +rel_imu_augmented_rmse,0.0 +rel_imu_augmented_orientation_rmse,0.0 +rel_imu_augmented_integrated_rmse,0.0 +rmse,0.0 +orientation_rmse,0.0 +integrated_rmse,0.0 +rel_rmse,0.0 +rel_orientation_rmse,0.0 +rel_integrated_rmse,0.0 +imu_augmented_rmse,0.0 +imu_augmented_orientation_rmse,0.0 +imu_augmented_integrated_rmse,0.0 +rel_imu_augmented_rmse,0.0 +rel_imu_augmented_orientation_rmse,0.0 +rel_imu_augmented_integrated_rmse,0.0 +rmse,0.0 +orientation_rmse,0.0 +integrated_rmse,0.0 +rel_rmse,0.0 +rel_orientation_rmse,0.0 +rel_integrated_rmse,0.0 +imu_augmented_rmse,0.0 +imu_augmented_orientation_rmse,0.0 +imu_augmented_integrated_rmse,0.0 +rel_imu_augmented_rmse,0.0 +rel_imu_augmented_orientation_rmse,0.0 +rel_imu_augmented_integrated_rmse,0.0 +rmse,0.0 +orientation_rmse,0.0 +integrated_rmse,0.0 +rel_rmse,0.0 +rel_orientation_rmse,0.0 +rel_integrated_rmse,0.0 +imu_augmented_rmse,0.0 +imu_augmented_orientation_rmse,0.0 +imu_augmented_integrated_rmse,0.0 +rel_imu_augmented_rmse,0.0 +rel_imu_augmented_orientation_rmse,0.0 +rel_imu_augmented_integrated_rmse,0.0 +rmse,0.0 +orientation_rmse,0.0 +integrated_rmse,0.0 +rel_rmse,0.0 +rel_orientation_rmse,0.0 +rel_integrated_rmse,0.0 +imu_augmented_rmse,0.0 +imu_augmented_orientation_rmse,0.0 +imu_augmented_integrated_rmse,0.0 +rel_imu_augmented_rmse,0.0 +rel_imu_augmented_orientation_rmse,0.0 +rel_imu_augmented_integrated_rmse,0.0 +rmse,0.0 +orientation_rmse,0.0 +integrated_rmse,0.0 +rel_rmse,0.0 +rel_orientation_rmse,0.0 +rel_integrated_rmse,0.0 +imu_augmented_rmse,0.0 +imu_augmented_orientation_rmse,0.0 +imu_augmented_integrated_rmse,0.0 +rel_imu_augmented_rmse,0.0 +rel_imu_augmented_orientation_rmse,0.0 +rel_imu_augmented_integrated_rmse,0.0 +rmse,0.0 +orientation_rmse,0.0 +integrated_rmse,0.0 +rel_rmse,0.0 +rel_orientation_rmse,0.0 +rel_integrated_rmse,0.0 +imu_augmented_rmse,0.0 +imu_augmented_orientation_rmse,0.0 +imu_augmented_integrated_rmse,0.0 +rel_imu_augmented_rmse,0.0 +rel_imu_augmented_orientation_rmse,0.0 +rel_imu_augmented_integrated_rmse,0.0 +rmse,0.0 +orientation_rmse,0.0 +integrated_rmse,0.0 +rel_rmse,0.0 +rel_orientation_rmse,0.0 +rel_integrated_rmse,0.0 +imu_augmented_rmse,0.0 +imu_augmented_orientation_rmse,0.0 +imu_augmented_integrated_rmse,0.0 +rel_imu_augmented_rmse,0.0 +rel_imu_augmented_orientation_rmse,0.0 +rel_imu_augmented_integrated_rmse,0.0 +rmse,0.0 +orientation_rmse,0.0 +integrated_rmse,0.0 +rel_rmse,0.0 +rel_orientation_rmse,0.0 +rel_integrated_rmse,0.0 +imu_augmented_rmse,0.0 +imu_augmented_orientation_rmse,0.0 +imu_augmented_integrated_rmse,0.0 +rel_imu_augmented_rmse,0.0 +rel_imu_augmented_orientation_rmse,0.0 +rel_imu_augmented_integrated_rmse,0.0 +rmse,0.0 +orientation_rmse,0.0 +integrated_rmse,0.0 +rel_rmse,0.0 +rel_orientation_rmse,0.0 +rel_integrated_rmse,0.0 +imu_augmented_rmse,0.0 +imu_augmented_orientation_rmse,0.0 +imu_augmented_integrated_rmse,0.0 +rel_imu_augmented_rmse,0.0 +rel_imu_augmented_orientation_rmse,0.0 +rel_imu_augmented_integrated_rmse,0.0 diff --git a/tools/localization_analysis/scripts/utilities.py b/tools/localization_analysis/scripts/utilities.py index dd391c9441..fec9c9b2a8 100644 --- a/tools/localization_analysis/scripts/utilities.py +++ b/tools/localization_analysis/scripts/utilities.py @@ -61,6 +61,9 @@ def integrate_velocities(localization_states): for velocity, delta_t in zip(localization_states.velocities.zs, delta_times) ] + if len(localization_states.times) == 0: + return localization_states + return add_increments_to_absolute_pose( x_increments, y_increments, diff --git a/tools/localization_analysis/scripts/vector3d_plotter.py b/tools/localization_analysis/scripts/vector3d_plotter.py index 620959709b..aa82b6c698 100644 --- a/tools/localization_analysis/scripts/vector3d_plotter.py +++ b/tools/localization_analysis/scripts/vector3d_plotter.py @@ -48,9 +48,11 @@ def add_pose_position( marker=None, markeredgewidth=None, markersize=1, + name=None, ): + name = pose.pose_type if name is None else name position_plotter = Vector3dYVals( - pose.pose_type, + name, pose.times, pose.positions.xs, pose.positions.ys, @@ -74,9 +76,11 @@ def add_pose_orientation( marker=None, markeredgewidth=None, markersize=1, - ): + name=None, + ): + name = pose.pose_type if name is None else name orientation_plotter = Vector3dYVals( - pose.pose_type, + name, pose.times, unwrap_in_degrees(pose.orientations.yaws), unwrap_in_degrees(pose.orientations.rolls), diff --git a/tools/localization_analysis/src/map_matcher.cc b/tools/localization_analysis/src/map_matcher.cc new file mode 100644 index 0000000000..6f56260e1a --- /dev/null +++ b/tools/localization_analysis/src/map_matcher.cc @@ -0,0 +1,111 @@ +/* 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. + */ + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +namespace localization_analysis { +namespace lc = localization_common; +namespace mc = msg_conversions; +MapMatcher::MapMatcher(const std::string& input_bag_name, const std::string& map_file, const std::string& image_topic, + const std::string& output_bag_name, const std::string& config_prefix, + const std::string& save_noloc_imgs) + : input_bag_(input_bag_name, rosbag::bagmode::Read), + output_bag_(output_bag_name, rosbag::bagmode::Write), + nonloc_bag_(), + image_topic_(image_topic), + map_(map_file, true), + map_feature_matcher_(&map_), + config_prefix_(config_prefix), + feature_averager_("Total number of features detected"), + match_count_(0), + image_count_(0) { + config_reader::ConfigReader config; + config.AddFile("geometry.config"); + lc::LoadGraphLocalizerConfig(config, config_prefix); + if (!config.ReadFiles()) { + LogFatal("Failed to read config files."); + } + body_T_nav_cam_ = lc::LoadTransform(config, "nav_cam_transform"); + sparse_mapping_min_num_landmarks_ = mc::LoadInt(config, "loc_adder_min_num_matches"); + if (!save_noloc_imgs.empty()) { + nonloc_bag_.open(save_noloc_imgs, rosbag::bagmode::Write); + } +} + +// TODO(rsoussan): Use common code with graph_bag +bool MapMatcher::GenerateVLFeatures(const sensor_msgs::ImageConstPtr& image_msg, + ff_msgs::VisualLandmarks& vl_features) { + // Convert image to cv image + cv_bridge::CvImageConstPtr image; + try { + image = cv_bridge::toCvShare(image_msg, sensor_msgs::image_encodings::MONO8); + } catch (cv_bridge::Exception& e) { + ROS_ERROR("cv_bridge exception: %s", e.what()); + return false; + } + + if (!map_feature_matcher_.Localize(image, &vl_features)) return false; + return true; +} + +void MapMatcher::AddMapMatches() { + std::vector topics; + topics.push_back(std::string("/") + image_topic_); + rosbag::View view(input_bag_, rosbag::TopicQuery(topics)); + image_count_ = view.size(); + for (const rosbag::MessageInstance msg : view) { + if (string_ends_with(msg.getTopic(), image_topic_)) { + sensor_msgs::ImageConstPtr image_msg = msg.instantiate(); + ff_msgs::VisualLandmarks vl_msg; + if (GenerateVLFeatures(image_msg, vl_msg)) { + match_count_++; + feature_averager_.Update(vl_msg.landmarks.size()); + const ros::Time timestamp = lc::RosTimeFromHeader(image_msg->header); + output_bag_.write(std::string("/") + TOPIC_LOCALIZATION_ML_FEATURES, timestamp, vl_msg); + if (graph_localizer::ValidVLMsg(vl_msg, sparse_mapping_min_num_landmarks_)) { + const gtsam::Pose3 sparse_mapping_global_T_body = + lc::PoseFromMsgWithExtrinsics(vl_msg.pose, body_T_nav_cam_.inverse()); + const auto pose_msg = + graph_localizer::PoseMsg(lc::EigenPose(sparse_mapping_global_T_body), lc::TimeFromHeader(vl_msg.header)); + output_bag_.write(std::string("/") + TOPIC_SPARSE_MAPPING_POSE, timestamp, pose_msg); + } + } else if (nonloc_bag_.isOpen()) { + const ros::Time timestamp = lc::RosTimeFromHeader(image_msg->header); + nonloc_bag_.write(std::string("/") + image_topic_, timestamp, image_msg); + } + } + } +} + +void MapMatcher::LogResults() { + std::stringstream ss; + ss << "Localized " << match_count_ << " / " << image_count_ << " images with mean of " << feature_averager_.average() + << " features"; + ROS_INFO_STREAM(ss.str()); +} +} // namespace localization_analysis diff --git a/tools/localization_analysis/tools/results.csv b/tools/localization_analysis/tools/results.csv new file mode 100644 index 0000000000..e10735170b --- /dev/null +++ b/tools/localization_analysis/tools/results.csv @@ -0,0 +1,36 @@ +rmse,0.02223592997076043 +orientation_rmse,0.014439809825783571 +integrated_rmse,14.535509517661424 +rel_rmse,0.02892175714834411 +rel_orientation_rmse,0.01689468156099405 +rel_integrated_rmse,0.02376021109303212 +imu_augmented_rmse,0.01909813576341352 +imu_augmented_orientation_rmse,0.016061346181761867 +imu_augmented_integrated_rmse,14.520322106346622 +rel_imu_augmented_rmse,0.029713320157916855 +rel_imu_augmented_orientation_rmse,0.01803531904369424 +rel_imu_augmented_integrated_rmse,0.025856844417313157 +rmse,0.0 +orientation_rmse,0.0 +integrated_rmse,0.0 +rel_rmse,0.0 +rel_orientation_rmse,0.0 +rel_integrated_rmse,0.0 +imu_augmented_rmse,0.0 +imu_augmented_orientation_rmse,0.0 +imu_augmented_integrated_rmse,0.0 +rel_imu_augmented_rmse,0.0 +rel_imu_augmented_orientation_rmse,0.0 +rel_imu_augmented_integrated_rmse,0.0 +rmse,0.0 +orientation_rmse,0.0 +integrated_rmse,0.0 +rel_rmse,0.0 +rel_orientation_rmse,0.0 +rel_integrated_rmse,0.0 +imu_augmented_rmse,0.0 +imu_augmented_orientation_rmse,0.0 +imu_augmented_integrated_rmse,0.0 +rel_imu_augmented_rmse,0.0 +rel_imu_augmented_orientation_rmse,0.0 +rel_imu_augmented_integrated_rmse,0.0 diff --git a/tools/localization_analysis/tools/run_map_matcher.cc b/tools/localization_analysis/tools/run_map_matcher.cc new file mode 100644 index 0000000000..de874070dd --- /dev/null +++ b/tools/localization_analysis/tools/run_map_matcher.cc @@ -0,0 +1,120 @@ +/* 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. + */ + +#include +#include +#include +#include + +#include +#include + +namespace po = boost::program_options; +namespace lc = localization_common; + +int main(int argc, char** argv) { + std::string output_bagfile; + std::string image_topic; + std::string robot_config_file; + std::string world; + std::string config_path_prefix; + std::string save_noloc_imgs; + std::string verbose; + std::string num_iterations; + po::options_description desc("Matches images to provided map and saves matches features and poses to a new bag file"); + desc.add_options()("help,h", "produce help message")("bagfile,b", po::value()->required(), + "Input bagfile containing image messages.")( + "map-file,m", po::value()->required(), "Map file")( + "config-path,c", po::value()->required(), "Path to config directory.")( + "image-topic,i", po::value(&image_topic)->default_value("mgt/img_sampler/nav_cam/image_record"), + "Image topic")("robot-config-file,r", + po::value(&robot_config_file)->default_value("config/robots/bumble.config"), + "Robot config file")("world,w", po::value(&world)->default_value("iss"), "World name")( + "output-bagfile,o", po::value(&output_bagfile)->default_value(""), + "Output bagfile, defaults to input_bag + _map_matches.bag")( + "config-path-prefix,p", po::value(&config_path_prefix)->default_value(""), "Config path prefix")( + "save-noloc-imgs,s", po::value(&save_noloc_imgs)->default_value("")->implicit_value(""), + "Save non-localized images to a bag, defaults to input_bag + _nonloc_imgs.bag")( + "verbose,v", po::value(&verbose)->default_value("")->implicit_value(""), "Verbose mode")( + "num_ransac_iterations,n", po::value(&num_iterations)->default_value("1000"), + "Number of RANSAC iterations"); + po::positional_options_description p; + p.add("bagfile", 1); + p.add("map-file", 1); + p.add("config-path", 1); + po::variables_map vm; + try { + po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm); + if (vm.count("help") || (argc <= 1)) { + std::cout << desc << "\n"; + return 1; + } + po::notify(vm); + } catch (std::exception& e) { + std::cerr << "Error: " << e.what() << "\n"; + return 1; + } + + const std::string input_bag = vm["bagfile"].as(); + const std::string map_file = vm["map-file"].as(); + const std::string config_path = vm["config-path"].as(); + + // Only pass program name to free flyer so that boost command line options + // are ignored when parsing gflags. + int ff_argc = 1; + ff_common::InitFreeFlyerApplication(&ff_argc, &argv); + + if (!boost::filesystem::exists(input_bag)) { + LogFatal("Bagfile " << input_bag << " not found."); + } + + if (!boost::filesystem::exists(map_file)) { + LogFatal("Map file " << map_file << " not found."); + } + + boost::filesystem::path input_bag_path(input_bag); + if (vm["output-bagfile"].defaulted()) { + boost::filesystem::path output_bag_path = + boost::filesystem::current_path() / boost::filesystem::path(input_bag_path.stem().string() + "_map_matches.bag"); + output_bagfile = output_bag_path.string(); + } + + if (!vm["save-noloc-imgs"].defaulted() && save_noloc_imgs.empty()) { + save_noloc_imgs = + boost::filesystem::current_path().string() + "/" + input_bag_path.stem().string() + "_nonloc_imgs.bag"; + } + + if (!vm["verbose"].defaulted() && !vm["verbose"].empty()) { + google::SetCommandLineOption("verbose_localization", "true"); + } + + if (!vm["num_ransac_iterations"].defaulted() && !vm["num_ransac_iterations"].empty()) { + google::SetCommandLineOption("num_ransac_iterations", num_iterations.data()); + } + + + + lc::SetEnvironmentConfigs(config_path, world, robot_config_file); + config_reader::ConfigReader config; + localization_analysis::MapMatcher map_matcher(input_bag, map_file, image_topic, output_bagfile, config_path_prefix, + save_noloc_imgs); + + map_matcher.AddMapMatches(); + ROS_INFO_STREAM("Using " << input_bag << " on map " << map_file); + map_matcher.LogResults(); +} diff --git a/tools/localization_analysis/tools/run_sparse_mapping_pose_adder.cc b/tools/localization_analysis/tools/run_sparse_mapping_pose_adder.cc index 5ff3069ea4..bec8977fc9 100644 --- a/tools/localization_analysis/tools/run_sparse_mapping_pose_adder.cc +++ b/tools/localization_analysis/tools/run_sparse_mapping_pose_adder.cc @@ -80,6 +80,6 @@ int main(int argc, char** argv) { const gtsam::Pose3 body_T_nav_cam = lc::LoadTransform(config, "nav_cam_transform"); localization_analysis::SparseMappingPoseAdder sparse_mapping_pose_adder(input_bag, output_bag_path.string(), - body_T_nav_cam.inverse()); + body_T_nav_cam.inverse()); sparse_mapping_pose_adder.AddPoses(); }