replace implicit use of cartographer::string with explicit use of std::string (#673)
Fixes #622.master
parent
291c0f581b
commit
f6192e4735
|
@ -27,14 +27,15 @@ namespace cartographer {
|
||||||
namespace common {
|
namespace common {
|
||||||
|
|
||||||
ConfigurationFileResolver::ConfigurationFileResolver(
|
ConfigurationFileResolver::ConfigurationFileResolver(
|
||||||
const std::vector<string>& configuration_files_directories)
|
const std::vector<std::string>& configuration_files_directories)
|
||||||
: configuration_files_directories_(configuration_files_directories) {
|
: configuration_files_directories_(configuration_files_directories) {
|
||||||
configuration_files_directories_.push_back(kConfigurationFilesDirectory);
|
configuration_files_directories_.push_back(kConfigurationFilesDirectory);
|
||||||
}
|
}
|
||||||
|
|
||||||
string ConfigurationFileResolver::GetFullPathOrDie(const string& basename) {
|
std::string ConfigurationFileResolver::GetFullPathOrDie(
|
||||||
|
const std::string& basename) {
|
||||||
for (const auto& path : configuration_files_directories_) {
|
for (const auto& path : configuration_files_directories_) {
|
||||||
const string filename = path + "/" + basename;
|
const std::string filename = path + "/" + basename;
|
||||||
std::ifstream stream(filename.c_str());
|
std::ifstream stream(filename.c_str());
|
||||||
if (stream.good()) {
|
if (stream.good()) {
|
||||||
LOG(INFO) << "Found '" << filename << "' for '" << basename << "'.";
|
LOG(INFO) << "Found '" << filename << "' for '" << basename << "'.";
|
||||||
|
@ -44,10 +45,11 @@ string ConfigurationFileResolver::GetFullPathOrDie(const string& basename) {
|
||||||
LOG(FATAL) << "File '" << basename << "' was not found.";
|
LOG(FATAL) << "File '" << basename << "' was not found.";
|
||||||
}
|
}
|
||||||
|
|
||||||
string ConfigurationFileResolver::GetFileContentOrDie(const string& basename) {
|
std::string ConfigurationFileResolver::GetFileContentOrDie(
|
||||||
const string filename = GetFullPathOrDie(basename);
|
const std::string& basename) {
|
||||||
|
const std::string filename = GetFullPathOrDie(basename);
|
||||||
std::ifstream stream(filename.c_str());
|
std::ifstream stream(filename.c_str());
|
||||||
return string((std::istreambuf_iterator<char>(stream)),
|
return std::string((std::istreambuf_iterator<char>(stream)),
|
||||||
std::istreambuf_iterator<char>());
|
std::istreambuf_iterator<char>());
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -34,13 +34,13 @@ namespace common {
|
||||||
class ConfigurationFileResolver : public FileResolver {
|
class ConfigurationFileResolver : public FileResolver {
|
||||||
public:
|
public:
|
||||||
explicit ConfigurationFileResolver(
|
explicit ConfigurationFileResolver(
|
||||||
const std::vector<string>& configuration_files_directories);
|
const std::vector<std::string>& configuration_files_directories);
|
||||||
|
|
||||||
string GetFullPathOrDie(const string& basename) override;
|
std::string GetFullPathOrDie(const std::string& basename) override;
|
||||||
string GetFileContentOrDie(const string& basename) override;
|
std::string GetFileContentOrDie(const std::string& basename) override;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
std::vector<string> configuration_files_directories_;
|
std::vector<std::string> configuration_files_directories_;
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace common
|
} // namespace common
|
||||||
|
|
|
@ -23,20 +23,19 @@
|
||||||
#include "cartographer/mapping/map_builder.h"
|
#include "cartographer/mapping/map_builder.h"
|
||||||
#include "gtest/gtest.h"
|
#include "gtest/gtest.h"
|
||||||
|
|
||||||
using std::string;
|
|
||||||
|
|
||||||
namespace cartographer_ros {
|
namespace cartographer_ros {
|
||||||
namespace {
|
namespace {
|
||||||
|
|
||||||
TEST(ConfigurationFilesTest, ValidateMapBuilderOptions) {
|
TEST(ConfigurationFilesTest, ValidateMapBuilderOptions) {
|
||||||
const string kCode = R"text(
|
const std::string kCode = R"text(
|
||||||
include "map_builder.lua"
|
include "map_builder.lua"
|
||||||
MAP_BUILDER.use_trajectory_builder_2d = true
|
MAP_BUILDER.use_trajectory_builder_2d = true
|
||||||
return MAP_BUILDER)text";
|
return MAP_BUILDER)text";
|
||||||
EXPECT_NO_FATAL_FAILURE({
|
EXPECT_NO_FATAL_FAILURE({
|
||||||
auto file_resolver = ::cartographer::common::make_unique<
|
auto file_resolver = ::cartographer::common::make_unique<
|
||||||
::cartographer::common::ConfigurationFileResolver>(
|
::cartographer::common::ConfigurationFileResolver>(
|
||||||
std::vector<string>{string(::cartographer::common::kSourceDirectory) +
|
std::vector<std::string>{
|
||||||
|
std::string(::cartographer::common::kSourceDirectory) +
|
||||||
"/configuration_files"});
|
"/configuration_files"});
|
||||||
::cartographer::common::LuaParameterDictionary lua_parameter_dictionary(
|
::cartographer::common::LuaParameterDictionary lua_parameter_dictionary(
|
||||||
kCode, std::move(file_resolver));
|
kCode, std::move(file_resolver));
|
||||||
|
@ -45,14 +44,15 @@ TEST(ConfigurationFilesTest, ValidateMapBuilderOptions) {
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(ConfigurationFilesTest, ValidateTrajectoryBuilderOptions) {
|
TEST(ConfigurationFilesTest, ValidateTrajectoryBuilderOptions) {
|
||||||
const string kCode = R"text(
|
const std::string kCode = R"text(
|
||||||
include "trajectory_builder.lua"
|
include "trajectory_builder.lua"
|
||||||
TRAJECTORY_BUILDER.trajectory_builder_2d.use_imu_data = false
|
TRAJECTORY_BUILDER.trajectory_builder_2d.use_imu_data = false
|
||||||
return TRAJECTORY_BUILDER)text";
|
return TRAJECTORY_BUILDER)text";
|
||||||
EXPECT_NO_FATAL_FAILURE({
|
EXPECT_NO_FATAL_FAILURE({
|
||||||
auto file_resolver = ::cartographer::common::make_unique<
|
auto file_resolver = ::cartographer::common::make_unique<
|
||||||
::cartographer::common::ConfigurationFileResolver>(
|
::cartographer::common::ConfigurationFileResolver>(
|
||||||
std::vector<string>{string(::cartographer::common::kSourceDirectory) +
|
std::vector<std::string>{
|
||||||
|
std::string(::cartographer::common::kSourceDirectory) +
|
||||||
"/configuration_files"});
|
"/configuration_files"});
|
||||||
::cartographer::common::LuaParameterDictionary lua_parameter_dictionary(
|
::cartographer::common::LuaParameterDictionary lua_parameter_dictionary(
|
||||||
kCode, std::move(file_resolver));
|
kCode, std::move(file_resolver));
|
||||||
|
|
|
@ -37,7 +37,7 @@ bool FixedRatioSampler::Pulse() {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
string FixedRatioSampler::DebugString() {
|
std::string FixedRatioSampler::DebugString() {
|
||||||
return std::to_string(num_samples_) + " (" +
|
return std::to_string(num_samples_) + " (" +
|
||||||
std::to_string(100. * num_samples_ / num_pulses_) + "%)";
|
std::to_string(100. * num_samples_ / num_pulses_) + "%)";
|
||||||
}
|
}
|
||||||
|
|
|
@ -38,7 +38,7 @@ class FixedRatioSampler {
|
||||||
bool Pulse();
|
bool Pulse();
|
||||||
|
|
||||||
// Returns a debug string describing the current ratio of samples to pulses.
|
// Returns a debug string describing the current ratio of samples to pulses.
|
||||||
string DebugString();
|
std::string DebugString();
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// Sampling occurs if the proportion of samples to pulses drops below this
|
// Sampling occurs if the proportion of samples to pulses drops below this
|
||||||
|
|
|
@ -28,7 +28,7 @@ namespace common {
|
||||||
|
|
||||||
void Histogram::Add(const float value) { values_.push_back(value); }
|
void Histogram::Add(const float value) { values_.push_back(value); }
|
||||||
|
|
||||||
string Histogram::ToString(const int buckets) const {
|
std::string Histogram::ToString(const int buckets) const {
|
||||||
CHECK_GE(buckets, 1);
|
CHECK_GE(buckets, 1);
|
||||||
if (values_.empty()) {
|
if (values_.empty()) {
|
||||||
return "Count: 0";
|
return "Count: 0";
|
||||||
|
@ -37,7 +37,7 @@ string Histogram::ToString(const int buckets) const {
|
||||||
const float max = *std::max_element(values_.begin(), values_.end());
|
const float max = *std::max_element(values_.begin(), values_.end());
|
||||||
const float mean =
|
const float mean =
|
||||||
std::accumulate(values_.begin(), values_.end(), 0.f) / values_.size();
|
std::accumulate(values_.begin(), values_.end(), 0.f) / values_.size();
|
||||||
string result = "Count: " + std::to_string(values_.size()) +
|
std::string result = "Count: " + std::to_string(values_.size()) +
|
||||||
" Min: " + std::to_string(min) +
|
" Min: " + std::to_string(min) +
|
||||||
" Max: " + std::to_string(max) +
|
" Max: " + std::to_string(max) +
|
||||||
" Mean: " + std::to_string(mean);
|
" Mean: " + std::to_string(mean);
|
||||||
|
|
|
@ -28,7 +28,7 @@ namespace common {
|
||||||
class Histogram {
|
class Histogram {
|
||||||
public:
|
public:
|
||||||
void Add(float value);
|
void Add(float value);
|
||||||
string ToString(int buckets) const;
|
std::string ToString(int buckets) const;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
std::vector<float> values_;
|
std::vector<float> values_;
|
||||||
|
|
|
@ -99,7 +99,7 @@ int LuaChoose(lua_State* L) {
|
||||||
|
|
||||||
// Pushes a value to the Lua stack.
|
// Pushes a value to the Lua stack.
|
||||||
void PushValue(lua_State* L, const int key) { lua_pushinteger(L, key); }
|
void PushValue(lua_State* L, const int key) { lua_pushinteger(L, key); }
|
||||||
void PushValue(lua_State* L, const string& key) {
|
void PushValue(lua_State* L, const std::string& key) {
|
||||||
lua_pushstring(L, key.c_str());
|
lua_pushstring(L, key.c_str());
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -147,18 +147,18 @@ void GetArrayValues(lua_State* L, const std::function<void()>& pop_value) {
|
||||||
|
|
||||||
std::unique_ptr<LuaParameterDictionary>
|
std::unique_ptr<LuaParameterDictionary>
|
||||||
LuaParameterDictionary::NonReferenceCounted(
|
LuaParameterDictionary::NonReferenceCounted(
|
||||||
const string& code, std::unique_ptr<FileResolver> file_resolver) {
|
const std::string& code, std::unique_ptr<FileResolver> file_resolver) {
|
||||||
return std::unique_ptr<LuaParameterDictionary>(new LuaParameterDictionary(
|
return std::unique_ptr<LuaParameterDictionary>(new LuaParameterDictionary(
|
||||||
code, ReferenceCount::NO, std::move(file_resolver)));
|
code, ReferenceCount::NO, std::move(file_resolver)));
|
||||||
}
|
}
|
||||||
|
|
||||||
LuaParameterDictionary::LuaParameterDictionary(
|
LuaParameterDictionary::LuaParameterDictionary(
|
||||||
const string& code, std::unique_ptr<FileResolver> file_resolver)
|
const std::string& code, std::unique_ptr<FileResolver> file_resolver)
|
||||||
: LuaParameterDictionary(code, ReferenceCount::YES,
|
: LuaParameterDictionary(code, ReferenceCount::YES,
|
||||||
std::move(file_resolver)) {}
|
std::move(file_resolver)) {}
|
||||||
|
|
||||||
LuaParameterDictionary::LuaParameterDictionary(
|
LuaParameterDictionary::LuaParameterDictionary(
|
||||||
const string& code, ReferenceCount reference_count,
|
const std::string& code, ReferenceCount reference_count,
|
||||||
std::unique_ptr<FileResolver> file_resolver)
|
std::unique_ptr<FileResolver> file_resolver)
|
||||||
: L_(luaL_newstate()),
|
: L_(luaL_newstate()),
|
||||||
index_into_reference_table_(-1),
|
index_into_reference_table_(-1),
|
||||||
|
@ -206,9 +206,9 @@ LuaParameterDictionary::~LuaParameterDictionary() {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
std::vector<string> LuaParameterDictionary::GetKeys() const {
|
std::vector<std::string> LuaParameterDictionary::GetKeys() const {
|
||||||
CheckTableIsAtTopOfStack(L_);
|
CheckTableIsAtTopOfStack(L_);
|
||||||
std::vector<string> keys;
|
std::vector<std::string> keys;
|
||||||
|
|
||||||
lua_pushnil(L_); // Push the first key
|
lua_pushnil(L_); // Push the first key
|
||||||
while (lua_next(L_, -2) != 0) {
|
while (lua_next(L_, -2) != 0) {
|
||||||
|
@ -220,28 +220,28 @@ std::vector<string> LuaParameterDictionary::GetKeys() const {
|
||||||
return keys;
|
return keys;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool LuaParameterDictionary::HasKey(const string& key) const {
|
bool LuaParameterDictionary::HasKey(const std::string& key) const {
|
||||||
return HasKeyOfType(L_, key);
|
return HasKeyOfType(L_, key);
|
||||||
}
|
}
|
||||||
|
|
||||||
string LuaParameterDictionary::GetString(const string& key) {
|
std::string LuaParameterDictionary::GetString(const std::string& key) {
|
||||||
CheckHasKeyAndReference(key);
|
CheckHasKeyAndReference(key);
|
||||||
GetValueFromLuaTable(L_, key);
|
GetValueFromLuaTable(L_, key);
|
||||||
return PopString(Quoted::NO);
|
return PopString(Quoted::NO);
|
||||||
}
|
}
|
||||||
|
|
||||||
string LuaParameterDictionary::PopString(Quoted quoted) const {
|
std::string LuaParameterDictionary::PopString(Quoted quoted) const {
|
||||||
CHECK(lua_isstring(L_, -1)) << "Top of stack is not a string value.";
|
CHECK(lua_isstring(L_, -1)) << "Top of stack is not a string value.";
|
||||||
if (quoted == Quoted::YES) {
|
if (quoted == Quoted::YES) {
|
||||||
QuoteStringOnStack(L_);
|
QuoteStringOnStack(L_);
|
||||||
}
|
}
|
||||||
|
|
||||||
const string value = lua_tostring(L_, -1);
|
const std::string value = lua_tostring(L_, -1);
|
||||||
lua_pop(L_, 1);
|
lua_pop(L_, 1);
|
||||||
return value;
|
return value;
|
||||||
}
|
}
|
||||||
|
|
||||||
double LuaParameterDictionary::GetDouble(const string& key) {
|
double LuaParameterDictionary::GetDouble(const std::string& key) {
|
||||||
CheckHasKeyAndReference(key);
|
CheckHasKeyAndReference(key);
|
||||||
GetValueFromLuaTable(L_, key);
|
GetValueFromLuaTable(L_, key);
|
||||||
return PopDouble();
|
return PopDouble();
|
||||||
|
@ -254,7 +254,7 @@ double LuaParameterDictionary::PopDouble() const {
|
||||||
return value;
|
return value;
|
||||||
}
|
}
|
||||||
|
|
||||||
int LuaParameterDictionary::GetInt(const string& key) {
|
int LuaParameterDictionary::GetInt(const std::string& key) {
|
||||||
CheckHasKeyAndReference(key);
|
CheckHasKeyAndReference(key);
|
||||||
GetValueFromLuaTable(L_, key);
|
GetValueFromLuaTable(L_, key);
|
||||||
return PopInt();
|
return PopInt();
|
||||||
|
@ -267,7 +267,7 @@ int LuaParameterDictionary::PopInt() const {
|
||||||
return value;
|
return value;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool LuaParameterDictionary::GetBool(const string& key) {
|
bool LuaParameterDictionary::GetBool(const std::string& key) {
|
||||||
CheckHasKeyAndReference(key);
|
CheckHasKeyAndReference(key);
|
||||||
GetValueFromLuaTable(L_, key);
|
GetValueFromLuaTable(L_, key);
|
||||||
return PopBool();
|
return PopBool();
|
||||||
|
@ -281,7 +281,7 @@ bool LuaParameterDictionary::PopBool() const {
|
||||||
}
|
}
|
||||||
|
|
||||||
std::unique_ptr<LuaParameterDictionary> LuaParameterDictionary::GetDictionary(
|
std::unique_ptr<LuaParameterDictionary> LuaParameterDictionary::GetDictionary(
|
||||||
const string& key) {
|
const std::string& key) {
|
||||||
CheckHasKeyAndReference(key);
|
CheckHasKeyAndReference(key);
|
||||||
GetValueFromLuaTable(L_, key);
|
GetValueFromLuaTable(L_, key);
|
||||||
return PopDictionary(reference_count_);
|
return PopDictionary(reference_count_);
|
||||||
|
@ -297,12 +297,13 @@ std::unique_ptr<LuaParameterDictionary> LuaParameterDictionary::PopDictionary(
|
||||||
return value;
|
return value;
|
||||||
}
|
}
|
||||||
|
|
||||||
string LuaParameterDictionary::DoToString(const string& indent) const {
|
std::string LuaParameterDictionary::DoToString(
|
||||||
string result = "{";
|
const std::string& indent) const {
|
||||||
|
std::string result = "{";
|
||||||
bool dictionary_is_empty = true;
|
bool dictionary_is_empty = true;
|
||||||
|
|
||||||
const auto top_of_stack_to_string = [this, indent,
|
const auto top_of_stack_to_string = [this, indent,
|
||||||
&dictionary_is_empty]() -> string {
|
&dictionary_is_empty]() -> std::string {
|
||||||
dictionary_is_empty = false;
|
dictionary_is_empty = false;
|
||||||
|
|
||||||
const int value_type = lua_type(L_, -1);
|
const int value_type = lua_type(L_, -1);
|
||||||
|
@ -346,10 +347,10 @@ string LuaParameterDictionary::DoToString(const string& indent) const {
|
||||||
}
|
}
|
||||||
|
|
||||||
// String keys.
|
// String keys.
|
||||||
std::vector<string> keys = GetKeys();
|
std::vector<std::string> keys = GetKeys();
|
||||||
if (!keys.empty()) {
|
if (!keys.empty()) {
|
||||||
std::sort(keys.begin(), keys.end());
|
std::sort(keys.begin(), keys.end());
|
||||||
for (const string& key : keys) {
|
for (const std::string& key : keys) {
|
||||||
GetValueFromLuaTable(L_, key);
|
GetValueFromLuaTable(L_, key);
|
||||||
result.append("\n");
|
result.append("\n");
|
||||||
result.append(indent);
|
result.append(indent);
|
||||||
|
@ -370,7 +371,7 @@ string LuaParameterDictionary::DoToString(const string& indent) const {
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
string LuaParameterDictionary::ToString() const { return DoToString(""); }
|
std::string LuaParameterDictionary::ToString() const { return DoToString(""); }
|
||||||
|
|
||||||
std::vector<double> LuaParameterDictionary::GetArrayValuesAsDoubles() {
|
std::vector<double> LuaParameterDictionary::GetArrayValuesAsDoubles() {
|
||||||
std::vector<double> values;
|
std::vector<double> values;
|
||||||
|
@ -387,19 +388,19 @@ LuaParameterDictionary::GetArrayValuesAsDictionaries() {
|
||||||
return values;
|
return values;
|
||||||
}
|
}
|
||||||
|
|
||||||
std::vector<string> LuaParameterDictionary::GetArrayValuesAsStrings() {
|
std::vector<std::string> LuaParameterDictionary::GetArrayValuesAsStrings() {
|
||||||
std::vector<string> values;
|
std::vector<std::string> values;
|
||||||
GetArrayValues(L_,
|
GetArrayValues(L_,
|
||||||
[&values, this] { values.push_back(PopString(Quoted::NO)); });
|
[&values, this] { values.push_back(PopString(Quoted::NO)); });
|
||||||
return values;
|
return values;
|
||||||
}
|
}
|
||||||
|
|
||||||
void LuaParameterDictionary::CheckHasKey(const string& key) const {
|
void LuaParameterDictionary::CheckHasKey(const std::string& key) const {
|
||||||
CHECK(HasKey(key)) << "Key '" << key << "' not in dictionary:\n"
|
CHECK(HasKey(key)) << "Key '" << key << "' not in dictionary:\n"
|
||||||
<< ToString();
|
<< ToString();
|
||||||
}
|
}
|
||||||
|
|
||||||
void LuaParameterDictionary::CheckHasKeyAndReference(const string& key) {
|
void LuaParameterDictionary::CheckHasKeyAndReference(const std::string& key) {
|
||||||
CheckHasKey(key);
|
CheckHasKey(key);
|
||||||
reference_counts_[key]++;
|
reference_counts_[key]++;
|
||||||
}
|
}
|
||||||
|
@ -414,7 +415,7 @@ void LuaParameterDictionary::CheckAllKeysWereUsedExactlyOnceAndReset() {
|
||||||
reference_counts_.clear();
|
reference_counts_.clear();
|
||||||
}
|
}
|
||||||
|
|
||||||
int LuaParameterDictionary::GetNonNegativeInt(const string& key) {
|
int LuaParameterDictionary::GetNonNegativeInt(const std::string& key) {
|
||||||
const int temp = GetInt(key); // Will increase reference count.
|
const int temp = GetInt(key); // Will increase reference count.
|
||||||
CHECK_GE(temp, 0) << temp << " is negative.";
|
CHECK_GE(temp, 0) << temp << " is negative.";
|
||||||
return temp;
|
return temp;
|
||||||
|
@ -427,15 +428,16 @@ int LuaParameterDictionary::LuaInclude(lua_State* L) {
|
||||||
CHECK(lua_isstring(L, -1)) << "include takes a filename.";
|
CHECK(lua_isstring(L, -1)) << "include takes a filename.";
|
||||||
|
|
||||||
LuaParameterDictionary* parameter_dictionary = GetDictionaryFromRegistry(L);
|
LuaParameterDictionary* parameter_dictionary = GetDictionaryFromRegistry(L);
|
||||||
const string basename = lua_tostring(L, -1);
|
const std::string basename = lua_tostring(L, -1);
|
||||||
const string filename =
|
const std::string filename =
|
||||||
parameter_dictionary->file_resolver_->GetFullPathOrDie(basename);
|
parameter_dictionary->file_resolver_->GetFullPathOrDie(basename);
|
||||||
if (std::find(parameter_dictionary->included_files_.begin(),
|
if (std::find(parameter_dictionary->included_files_.begin(),
|
||||||
parameter_dictionary->included_files_.end(),
|
parameter_dictionary->included_files_.end(),
|
||||||
filename) != parameter_dictionary->included_files_.end()) {
|
filename) != parameter_dictionary->included_files_.end()) {
|
||||||
string error_msg = "Tried to include " + filename +
|
std::string error_msg =
|
||||||
|
"Tried to include " + filename +
|
||||||
" twice. Already included files in order of inclusion: ";
|
" twice. Already included files in order of inclusion: ";
|
||||||
for (const string& filename : parameter_dictionary->included_files_) {
|
for (const std::string& filename : parameter_dictionary->included_files_) {
|
||||||
error_msg.append(filename);
|
error_msg.append(filename);
|
||||||
error_msg.append("\n");
|
error_msg.append("\n");
|
||||||
}
|
}
|
||||||
|
@ -445,7 +447,7 @@ int LuaParameterDictionary::LuaInclude(lua_State* L) {
|
||||||
lua_pop(L, 1);
|
lua_pop(L, 1);
|
||||||
CHECK_EQ(lua_gettop(L), 0);
|
CHECK_EQ(lua_gettop(L), 0);
|
||||||
|
|
||||||
const string content =
|
const std::string content =
|
||||||
parameter_dictionary->file_resolver_->GetFileContentOrDie(basename);
|
parameter_dictionary->file_resolver_->GetFileContentOrDie(basename);
|
||||||
CheckForLuaErrors(
|
CheckForLuaErrors(
|
||||||
L, luaL_loadbuffer(L, content.c_str(), content.size(), filename.c_str()));
|
L, luaL_loadbuffer(L, content.c_str(), content.size(), filename.c_str()));
|
||||||
|
@ -460,7 +462,7 @@ int LuaParameterDictionary::LuaRead(lua_State* L) {
|
||||||
CHECK(lua_isstring(L, -1)) << "read takes a filename.";
|
CHECK(lua_isstring(L, -1)) << "read takes a filename.";
|
||||||
|
|
||||||
LuaParameterDictionary* parameter_dictionary = GetDictionaryFromRegistry(L);
|
LuaParameterDictionary* parameter_dictionary = GetDictionaryFromRegistry(L);
|
||||||
const string file_content =
|
const std::string file_content =
|
||||||
parameter_dictionary->file_resolver_->GetFileContentOrDie(
|
parameter_dictionary->file_resolver_->GetFileContentOrDie(
|
||||||
lua_tostring(L, -1));
|
lua_tostring(L, -1));
|
||||||
lua_pushstring(L, file_content.c_str());
|
lua_pushstring(L, file_content.c_str());
|
||||||
|
|
|
@ -34,15 +34,15 @@ namespace common {
|
||||||
class FileResolver {
|
class FileResolver {
|
||||||
public:
|
public:
|
||||||
virtual ~FileResolver() {}
|
virtual ~FileResolver() {}
|
||||||
virtual string GetFullPathOrDie(const string& basename) = 0;
|
virtual std::string GetFullPathOrDie(const std::string& basename) = 0;
|
||||||
virtual string GetFileContentOrDie(const string& basename) = 0;
|
virtual std::string GetFileContentOrDie(const std::string& basename) = 0;
|
||||||
};
|
};
|
||||||
|
|
||||||
// A parameter dictionary that gets loaded from Lua code.
|
// A parameter dictionary that gets loaded from Lua code.
|
||||||
class LuaParameterDictionary {
|
class LuaParameterDictionary {
|
||||||
public:
|
public:
|
||||||
// Constructs the dictionary from a Lua Table specification.
|
// Constructs the dictionary from a Lua Table specification.
|
||||||
LuaParameterDictionary(const string& code,
|
LuaParameterDictionary(const std::string& code,
|
||||||
std::unique_ptr<FileResolver> file_resolver);
|
std::unique_ptr<FileResolver> file_resolver);
|
||||||
|
|
||||||
LuaParameterDictionary(const LuaParameterDictionary&) = delete;
|
LuaParameterDictionary(const LuaParameterDictionary&) = delete;
|
||||||
|
@ -50,38 +50,39 @@ class LuaParameterDictionary {
|
||||||
|
|
||||||
// Constructs a LuaParameterDictionary without reference counting.
|
// Constructs a LuaParameterDictionary without reference counting.
|
||||||
static std::unique_ptr<LuaParameterDictionary> NonReferenceCounted(
|
static std::unique_ptr<LuaParameterDictionary> NonReferenceCounted(
|
||||||
const string& code, std::unique_ptr<FileResolver> file_resolver);
|
const std::string& code, std::unique_ptr<FileResolver> file_resolver);
|
||||||
|
|
||||||
~LuaParameterDictionary();
|
~LuaParameterDictionary();
|
||||||
|
|
||||||
// Returns all available keys.
|
// Returns all available keys.
|
||||||
std::vector<string> GetKeys() const;
|
std::vector<std::string> GetKeys() const;
|
||||||
|
|
||||||
// Returns true if the key is in this dictionary.
|
// Returns true if the key is in this dictionary.
|
||||||
bool HasKey(const string& key) const;
|
bool HasKey(const std::string& key) const;
|
||||||
|
|
||||||
// These methods CHECK() that the 'key' exists.
|
// These methods CHECK() that the 'key' exists.
|
||||||
string GetString(const string& key);
|
std::string GetString(const std::string& key);
|
||||||
double GetDouble(const string& key);
|
double GetDouble(const std::string& key);
|
||||||
int GetInt(const string& key);
|
int GetInt(const std::string& key);
|
||||||
bool GetBool(const string& key);
|
bool GetBool(const std::string& key);
|
||||||
std::unique_ptr<LuaParameterDictionary> GetDictionary(const string& key);
|
std::unique_ptr<LuaParameterDictionary> GetDictionary(const std::string& key);
|
||||||
|
|
||||||
// Gets an int from the dictionary and CHECK()s that it is non-negative.
|
// Gets an int from the dictionary and CHECK()s that it is non-negative.
|
||||||
int GetNonNegativeInt(const string& key);
|
int GetNonNegativeInt(const std::string& key);
|
||||||
|
|
||||||
// Returns a string representation for this LuaParameterDictionary.
|
// Returns a string representation for this LuaParameterDictionary.
|
||||||
string ToString() const;
|
std::string ToString() const;
|
||||||
|
|
||||||
// Returns the values of the keys '1', '2', '3' as the given types.
|
// Returns the values of the keys '1', '2', '3' as the given types.
|
||||||
std::vector<double> GetArrayValuesAsDoubles();
|
std::vector<double> GetArrayValuesAsDoubles();
|
||||||
std::vector<string> GetArrayValuesAsStrings();
|
std::vector<std::string> GetArrayValuesAsStrings();
|
||||||
std::vector<std::unique_ptr<LuaParameterDictionary>>
|
std::vector<std::unique_ptr<LuaParameterDictionary>>
|
||||||
GetArrayValuesAsDictionaries();
|
GetArrayValuesAsDictionaries();
|
||||||
|
|
||||||
private:
|
private:
|
||||||
enum class ReferenceCount { YES, NO };
|
enum class ReferenceCount { YES, NO };
|
||||||
LuaParameterDictionary(const string& code, ReferenceCount reference_count,
|
LuaParameterDictionary(const std::string& code,
|
||||||
|
ReferenceCount reference_count,
|
||||||
std::unique_ptr<FileResolver> file_resolver);
|
std::unique_ptr<FileResolver> file_resolver);
|
||||||
|
|
||||||
// For GetDictionary().
|
// For GetDictionary().
|
||||||
|
@ -89,7 +90,7 @@ class LuaParameterDictionary {
|
||||||
std::shared_ptr<FileResolver> file_resolver);
|
std::shared_ptr<FileResolver> file_resolver);
|
||||||
|
|
||||||
// Function that recurses to keep track of indent for ToString().
|
// Function that recurses to keep track of indent for ToString().
|
||||||
string DoToString(const string& indent) const;
|
std::string DoToString(const std::string& indent) const;
|
||||||
|
|
||||||
// Pop the top of the stack and CHECKs that the type is correct.
|
// Pop the top of the stack and CHECKs that the type is correct.
|
||||||
double PopDouble() const;
|
double PopDouble() const;
|
||||||
|
@ -100,7 +101,7 @@ class LuaParameterDictionary {
|
||||||
// is either quoted to be suitable to be read back by a Lua interpretor or
|
// is either quoted to be suitable to be read back by a Lua interpretor or
|
||||||
// not.
|
// not.
|
||||||
enum class Quoted { YES, NO };
|
enum class Quoted { YES, NO };
|
||||||
string PopString(Quoted quoted) const;
|
std::string PopString(Quoted quoted) const;
|
||||||
|
|
||||||
// Creates a LuaParameterDictionary from the Lua table at the top of the
|
// Creates a LuaParameterDictionary from the Lua table at the top of the
|
||||||
// stack, either with or without reference counting.
|
// stack, either with or without reference counting.
|
||||||
|
@ -108,10 +109,10 @@ class LuaParameterDictionary {
|
||||||
ReferenceCount reference_count) const;
|
ReferenceCount reference_count) const;
|
||||||
|
|
||||||
// CHECK() that 'key' is in the dictionary.
|
// CHECK() that 'key' is in the dictionary.
|
||||||
void CheckHasKey(const string& key) const;
|
void CheckHasKey(const std::string& key) const;
|
||||||
|
|
||||||
// CHECK() that 'key' is in this dictionary and reference it as being used.
|
// CHECK() that 'key' is in this dictionary and reference it as being used.
|
||||||
void CheckHasKeyAndReference(const string& key);
|
void CheckHasKeyAndReference(const std::string& key);
|
||||||
|
|
||||||
// If desired, this can be called in the destructor of a derived class. It
|
// If desired, this can be called in the destructor of a derived class. It
|
||||||
// will CHECK() that all keys defined in the configuration have been used
|
// will CHECK() that all keys defined in the configuration have been used
|
||||||
|
@ -135,11 +136,11 @@ class LuaParameterDictionary {
|
||||||
|
|
||||||
// This is modified with every call to Get* in order to verify that all
|
// This is modified with every call to Get* in order to verify that all
|
||||||
// parameters are read exactly once.
|
// parameters are read exactly once.
|
||||||
std::map<string, int> reference_counts_;
|
std::map<std::string, int> reference_counts_;
|
||||||
|
|
||||||
// List of all included files in order of inclusion. Used to prevent double
|
// List of all included files in order of inclusion. Used to prevent double
|
||||||
// inclusion.
|
// inclusion.
|
||||||
std::vector<string> included_files_;
|
std::vector<std::string> included_files_;
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace common
|
} // namespace common
|
||||||
|
|
|
@ -29,7 +29,7 @@ namespace common {
|
||||||
namespace {
|
namespace {
|
||||||
|
|
||||||
std::unique_ptr<LuaParameterDictionary> MakeNonReferenceCounted(
|
std::unique_ptr<LuaParameterDictionary> MakeNonReferenceCounted(
|
||||||
const string& code) {
|
const std::string& code) {
|
||||||
return LuaParameterDictionary::NonReferenceCounted(
|
return LuaParameterDictionary::NonReferenceCounted(
|
||||||
code, common::make_unique<DummyFileResolver>());
|
code, common::make_unique<DummyFileResolver>());
|
||||||
}
|
}
|
||||||
|
@ -37,7 +37,7 @@ std::unique_ptr<LuaParameterDictionary> MakeNonReferenceCounted(
|
||||||
class LuaParameterDictionaryTest : public ::testing::Test {
|
class LuaParameterDictionaryTest : public ::testing::Test {
|
||||||
protected:
|
protected:
|
||||||
void ReferenceAllKeysAsIntegers(LuaParameterDictionary* dict) {
|
void ReferenceAllKeysAsIntegers(LuaParameterDictionary* dict) {
|
||||||
for (const string& key : dict->GetKeys()) {
|
for (const std::string& key : dict->GetKeys()) {
|
||||||
dict->GetInt(key);
|
dict->GetInt(key);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -73,7 +73,7 @@ TEST_F(LuaParameterDictionaryTest, GetDictionary) {
|
||||||
MakeDictionary("return { blah = { blue = 100, red = 200 }, fasel = 10 }");
|
MakeDictionary("return { blah = { blue = 100, red = 200 }, fasel = 10 }");
|
||||||
|
|
||||||
std::unique_ptr<LuaParameterDictionary> sub_dict(dict->GetDictionary("blah"));
|
std::unique_ptr<LuaParameterDictionary> sub_dict(dict->GetDictionary("blah"));
|
||||||
std::vector<string> keys = sub_dict->GetKeys();
|
std::vector<std::string> keys = sub_dict->GetKeys();
|
||||||
ASSERT_EQ(keys.size(), 2);
|
ASSERT_EQ(keys.size(), 2);
|
||||||
std::sort(keys.begin(), keys.end());
|
std::sort(keys.begin(), keys.end());
|
||||||
ASSERT_EQ(keys[0], "blue");
|
ASSERT_EQ(keys[0], "blue");
|
||||||
|
@ -89,7 +89,7 @@ TEST_F(LuaParameterDictionaryTest, GetDictionary) {
|
||||||
TEST_F(LuaParameterDictionaryTest, GetKeys) {
|
TEST_F(LuaParameterDictionaryTest, GetKeys) {
|
||||||
auto dict = MakeDictionary("return { blah = 100, blah1 = 200}");
|
auto dict = MakeDictionary("return { blah = 100, blah1 = 200}");
|
||||||
|
|
||||||
std::vector<string> keys = dict->GetKeys();
|
std::vector<std::string> keys = dict->GetKeys();
|
||||||
ASSERT_EQ(keys.size(), 2);
|
ASSERT_EQ(keys.size(), 2);
|
||||||
std::sort(keys.begin(), keys.end());
|
std::sort(keys.begin(), keys.end());
|
||||||
ASSERT_EQ(keys[0], "blah");
|
ASSERT_EQ(keys[0], "blah");
|
||||||
|
@ -122,7 +122,7 @@ TEST_F(LuaParameterDictionaryTest, ToString) {
|
||||||
alpha1 = false,
|
alpha1 = false,
|
||||||
})");
|
})");
|
||||||
|
|
||||||
const string golden = R"({
|
const std::string golden = R"({
|
||||||
alpha = true,
|
alpha = true,
|
||||||
alpha1 = false,
|
alpha1 = false,
|
||||||
blub = "hello",
|
blub = "hello",
|
||||||
|
@ -164,7 +164,7 @@ TEST_F(LuaParameterDictionaryTest, ToStringForArrays) {
|
||||||
foo = "ups",
|
foo = "ups",
|
||||||
})");
|
})");
|
||||||
|
|
||||||
const string golden = R"({
|
const std::string golden = R"({
|
||||||
"blub",
|
"blub",
|
||||||
3.000000,
|
3.000000,
|
||||||
3.100000,
|
3.100000,
|
||||||
|
@ -176,7 +176,7 @@ TEST_F(LuaParameterDictionaryTest, ToStringForArrays) {
|
||||||
TEST_F(LuaParameterDictionaryTest, GetArrayValuesAsStrings) {
|
TEST_F(LuaParameterDictionaryTest, GetArrayValuesAsStrings) {
|
||||||
auto dict = MakeDictionary("return { 'a', 'b', 'c' }");
|
auto dict = MakeDictionary("return { 'a', 'b', 'c' }");
|
||||||
EXPECT_EQ(0, dict->GetKeys().size());
|
EXPECT_EQ(0, dict->GetKeys().size());
|
||||||
const std::vector<string> values = dict->GetArrayValuesAsStrings();
|
const std::vector<std::string> values = dict->GetArrayValuesAsStrings();
|
||||||
EXPECT_EQ(3, values.size());
|
EXPECT_EQ(3, values.size());
|
||||||
EXPECT_EQ("a", values[0]);
|
EXPECT_EQ("a", values[0]);
|
||||||
EXPECT_EQ("b", values[1]);
|
EXPECT_EQ("b", values[1]);
|
||||||
|
|
|
@ -37,16 +37,17 @@ class DummyFileResolver : public FileResolver {
|
||||||
|
|
||||||
~DummyFileResolver() override {}
|
~DummyFileResolver() override {}
|
||||||
|
|
||||||
string GetFileContentOrDie(const string& unused_basename) override {
|
std::string GetFileContentOrDie(const std::string& unused_basename) override {
|
||||||
LOG(FATAL) << "Not implemented";
|
LOG(FATAL) << "Not implemented";
|
||||||
}
|
}
|
||||||
|
|
||||||
string GetFullPathOrDie(const string& unused_basename) override {
|
std::string GetFullPathOrDie(const std::string& unused_basename) override {
|
||||||
LOG(FATAL) << "Not implemented";
|
LOG(FATAL) << "Not implemented";
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
std::unique_ptr<LuaParameterDictionary> MakeDictionary(const string& code) {
|
std::unique_ptr<LuaParameterDictionary> MakeDictionary(
|
||||||
|
const std::string& code) {
|
||||||
return common::make_unique<LuaParameterDictionary>(
|
return common::make_unique<LuaParameterDictionary>(
|
||||||
code, common::make_unique<DummyFileResolver>());
|
code, common::make_unique<DummyFileResolver>());
|
||||||
}
|
}
|
||||||
|
|
|
@ -36,8 +36,6 @@ using uint16 = uint16_t;
|
||||||
using uint32 = uint32_t;
|
using uint32 = uint32_t;
|
||||||
using uint64 = uint64_t;
|
using uint64 = uint64_t;
|
||||||
|
|
||||||
using std::string;
|
|
||||||
|
|
||||||
namespace common {
|
namespace common {
|
||||||
|
|
||||||
inline int RoundToInt(const float x) { return std::lround(x); }
|
inline int RoundToInt(const float x) { return std::lround(x); }
|
||||||
|
@ -48,7 +46,8 @@ inline int64 RoundToInt64(const float x) { return std::lround(x); }
|
||||||
|
|
||||||
inline int64 RoundToInt64(const double x) { return std::lround(x); }
|
inline int64 RoundToInt64(const double x) { return std::lround(x); }
|
||||||
|
|
||||||
inline void FastGzipString(const string& uncompressed, string* compressed) {
|
inline void FastGzipString(const std::string& uncompressed,
|
||||||
|
std::string* compressed) {
|
||||||
boost::iostreams::filtering_ostream out;
|
boost::iostreams::filtering_ostream out;
|
||||||
out.push(
|
out.push(
|
||||||
boost::iostreams::gzip_compressor(boost::iostreams::zlib::best_speed));
|
boost::iostreams::gzip_compressor(boost::iostreams::zlib::best_speed));
|
||||||
|
@ -58,7 +57,8 @@ inline void FastGzipString(const string& uncompressed, string* compressed) {
|
||||||
uncompressed.size());
|
uncompressed.size());
|
||||||
}
|
}
|
||||||
|
|
||||||
inline void FastGunzipString(const string& compressed, string* decompressed) {
|
inline void FastGunzipString(const std::string& compressed,
|
||||||
|
std::string* decompressed) {
|
||||||
boost::iostreams::filtering_ostream out;
|
boost::iostreams::filtering_ostream out;
|
||||||
out.push(boost::iostreams::gzip_decompressor());
|
out.push(boost::iostreams::gzip_decompressor());
|
||||||
out.push(boost::iostreams::back_inserter(*decompressed));
|
out.push(boost::iostreams::back_inserter(*decompressed));
|
||||||
|
|
|
@ -78,7 +78,7 @@ class RateTimer {
|
||||||
}
|
}
|
||||||
|
|
||||||
// Returns a debug string representation.
|
// Returns a debug string representation.
|
||||||
string DebugString() const {
|
std::string DebugString() const {
|
||||||
if (events_.size() < 2) {
|
if (events_.size() < 2) {
|
||||||
return "unknown";
|
return "unknown";
|
||||||
}
|
}
|
||||||
|
@ -109,7 +109,7 @@ class RateTimer {
|
||||||
}
|
}
|
||||||
|
|
||||||
// Returns the average and standard deviation of the deltas.
|
// Returns the average and standard deviation of the deltas.
|
||||||
string DeltasDebugString() const {
|
std::string DeltasDebugString() const {
|
||||||
const auto deltas = ComputeDeltasInSeconds();
|
const auto deltas = ComputeDeltasInSeconds();
|
||||||
const double sum = std::accumulate(deltas.begin(), deltas.end(), 0.);
|
const double sum = std::accumulate(deltas.begin(), deltas.end(), 0.);
|
||||||
const double mean = sum / deltas.size();
|
const double mean = sum / deltas.size();
|
||||||
|
|
|
@ -163,8 +163,8 @@ proto::GroundTruth GenerateGroundTruth(
|
||||||
return ground_truth;
|
return ground_truth;
|
||||||
}
|
}
|
||||||
|
|
||||||
void Run(const string& pose_graph_filename, const string& output_filename,
|
void Run(const std::string& pose_graph_filename,
|
||||||
const double min_covered_distance,
|
const std::string& output_filename, const double min_covered_distance,
|
||||||
const double outlier_threshold_meters,
|
const double outlier_threshold_meters,
|
||||||
const double outlier_threshold_radians) {
|
const double outlier_threshold_radians) {
|
||||||
LOG(INFO) << "Reading pose graph from '" << pose_graph_filename << "'...";
|
LOG(INFO) << "Reading pose graph from '" << pose_graph_filename << "'...";
|
||||||
|
|
|
@ -67,7 +67,7 @@ Error ComputeError(const transform::Rigid3d& pose1,
|
||||||
common::Pow2(transform::GetAngle(error))};
|
common::Pow2(transform::GetAngle(error))};
|
||||||
}
|
}
|
||||||
|
|
||||||
string MeanAndStdDevString(const std::vector<double>& values) {
|
std::string MeanAndStdDevString(const std::vector<double>& values) {
|
||||||
CHECK_GE(values.size(), 2);
|
CHECK_GE(values.size(), 2);
|
||||||
const double mean =
|
const double mean =
|
||||||
std::accumulate(values.begin(), values.end(), 0.) / values.size();
|
std::accumulate(values.begin(), values.end(), 0.) / values.size();
|
||||||
|
@ -80,10 +80,10 @@ string MeanAndStdDevString(const std::vector<double>& values) {
|
||||||
std::ostringstream out;
|
std::ostringstream out;
|
||||||
out << std::fixed << std::setprecision(5) << mean << " +/- "
|
out << std::fixed << std::setprecision(5) << mean << " +/- "
|
||||||
<< standard_deviation;
|
<< standard_deviation;
|
||||||
return string(out.str());
|
return std::string(out.str());
|
||||||
}
|
}
|
||||||
|
|
||||||
string StatisticsString(const std::vector<Error>& errors) {
|
std::string StatisticsString(const std::vector<Error>& errors) {
|
||||||
std::vector<double> translational_errors;
|
std::vector<double> translational_errors;
|
||||||
std::vector<double> squared_translational_errors;
|
std::vector<double> squared_translational_errors;
|
||||||
std::vector<double> rotational_errors_degrees;
|
std::vector<double> rotational_errors_degrees;
|
||||||
|
@ -109,7 +109,8 @@ string StatisticsString(const std::vector<Error>& errors) {
|
||||||
MeanAndStdDevString(squared_rotational_errors_degrees) + " deg^2\n";
|
MeanAndStdDevString(squared_rotational_errors_degrees) + " deg^2\n";
|
||||||
}
|
}
|
||||||
|
|
||||||
void Run(const string& pose_graph_filename, const string& relations_filename,
|
void Run(const std::string& pose_graph_filename,
|
||||||
|
const std::string& relations_filename,
|
||||||
const bool read_text_file_with_unix_timestamps) {
|
const bool read_text_file_with_unix_timestamps) {
|
||||||
LOG(INFO) << "Reading pose graph from '" << pose_graph_filename << "'...";
|
LOG(INFO) << "Reading pose graph from '" << pose_graph_filename << "'...";
|
||||||
mapping::proto::SparsePoseGraph pose_graph;
|
mapping::proto::SparsePoseGraph pose_graph;
|
||||||
|
|
|
@ -37,7 +37,8 @@ common::Time UnixToCommonTime(double unix_time) {
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
|
|
||||||
proto::GroundTruth ReadRelationsTextFile(const string& relations_filename) {
|
proto::GroundTruth ReadRelationsTextFile(
|
||||||
|
const std::string& relations_filename) {
|
||||||
proto::GroundTruth ground_truth;
|
proto::GroundTruth ground_truth;
|
||||||
std::ifstream relations_stream(relations_filename.c_str());
|
std::ifstream relations_stream(relations_filename.c_str());
|
||||||
double unix_time_1, unix_time_2, x, y, z, roll, pitch, yaw;
|
double unix_time_1, unix_time_2, x, y, z, roll, pitch, yaw;
|
||||||
|
|
|
@ -33,7 +33,7 @@ namespace ground_truth {
|
||||||
// R. Kuemmerle, B. Steder, C. Dornhege, M. Ruhnke, G. Grisetti, C. Stachniss,
|
// R. Kuemmerle, B. Steder, C. Dornhege, M. Ruhnke, G. Grisetti, C. Stachniss,
|
||||||
// and A. Kleiner, "On measuring the accuracy of SLAM algorithms," Autonomous
|
// and A. Kleiner, "On measuring the accuracy of SLAM algorithms," Autonomous
|
||||||
// Robots, vol. 27, no. 4, pp. 387–407, 2009.
|
// Robots, vol. 27, no. 4, pp. 387–407, 2009.
|
||||||
proto::GroundTruth ReadRelationsTextFile(const string& relations_filename);
|
proto::GroundTruth ReadRelationsTextFile(const std::string& relations_filename);
|
||||||
|
|
||||||
} // namespace ground_truth
|
} // namespace ground_truth
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
||||||
|
|
|
@ -27,7 +27,7 @@ std::unique_ptr<ColoringPointsProcessor>
|
||||||
ColoringPointsProcessor::FromDictionary(
|
ColoringPointsProcessor::FromDictionary(
|
||||||
common::LuaParameterDictionary* const dictionary,
|
common::LuaParameterDictionary* const dictionary,
|
||||||
PointsProcessor* const next) {
|
PointsProcessor* const next) {
|
||||||
const string frame_id = dictionary->GetString("frame_id");
|
const std::string frame_id = dictionary->GetString("frame_id");
|
||||||
const std::vector<double> color_values =
|
const std::vector<double> color_values =
|
||||||
dictionary->GetDictionary("color")->GetArrayValuesAsDoubles();
|
dictionary->GetDictionary("color")->GetArrayValuesAsDoubles();
|
||||||
const Uint8Color color = {{static_cast<uint8>(color_values[0]),
|
const Uint8Color color = {{static_cast<uint8>(color_values[0]),
|
||||||
|
@ -38,7 +38,7 @@ ColoringPointsProcessor::FromDictionary(
|
||||||
}
|
}
|
||||||
|
|
||||||
ColoringPointsProcessor::ColoringPointsProcessor(const FloatColor& color,
|
ColoringPointsProcessor::ColoringPointsProcessor(const FloatColor& color,
|
||||||
const string& frame_id,
|
const std::string& frame_id,
|
||||||
PointsProcessor* const next)
|
PointsProcessor* const next)
|
||||||
: color_(color), frame_id_(frame_id), next_(next) {}
|
: color_(color), frame_id_(frame_id), next_(next) {}
|
||||||
|
|
||||||
|
|
|
@ -31,7 +31,7 @@ class ColoringPointsProcessor : public PointsProcessor {
|
||||||
public:
|
public:
|
||||||
constexpr static const char* kConfigurationFileActionName = "color_points";
|
constexpr static const char* kConfigurationFileActionName = "color_points";
|
||||||
|
|
||||||
ColoringPointsProcessor(const FloatColor& color, const string& frame_id,
|
ColoringPointsProcessor(const FloatColor& color, const std::string& frame_id,
|
||||||
PointsProcessor* next);
|
PointsProcessor* next);
|
||||||
|
|
||||||
static std::unique_ptr<ColoringPointsProcessor> FromDictionary(
|
static std::unique_ptr<ColoringPointsProcessor> FromDictionary(
|
||||||
|
@ -47,7 +47,7 @@ class ColoringPointsProcessor : public PointsProcessor {
|
||||||
|
|
||||||
private:
|
private:
|
||||||
const FloatColor color_;
|
const FloatColor color_;
|
||||||
const string frame_id_;
|
const std::string frame_id_;
|
||||||
PointsProcessor* const next_;
|
PointsProcessor* const next_;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -19,7 +19,7 @@
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
namespace io {
|
namespace io {
|
||||||
|
|
||||||
StreamFileWriter::StreamFileWriter(const string& filename)
|
StreamFileWriter::StreamFileWriter(const std::string& filename)
|
||||||
: filename_(filename), out_(filename, std::ios::out | std::ios::binary) {}
|
: filename_(filename), out_(filename, std::ios::out | std::ios::binary) {}
|
||||||
|
|
||||||
StreamFileWriter::~StreamFileWriter() {}
|
StreamFileWriter::~StreamFileWriter() {}
|
||||||
|
@ -49,7 +49,7 @@ bool StreamFileWriter::WriteHeader(const char* const data, const size_t len) {
|
||||||
return Write(data, len);
|
return Write(data, len);
|
||||||
}
|
}
|
||||||
|
|
||||||
string StreamFileWriter::GetFilename() { return filename_; }
|
std::string StreamFileWriter::GetFilename() { return filename_; }
|
||||||
|
|
||||||
} // namespace io
|
} // namespace io
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
||||||
|
|
|
@ -42,7 +42,7 @@ class FileWriter {
|
||||||
|
|
||||||
virtual bool Write(const char* data, size_t len) = 0;
|
virtual bool Write(const char* data, size_t len) = 0;
|
||||||
virtual bool Close() = 0;
|
virtual bool Close() = 0;
|
||||||
virtual string GetFilename() = 0;
|
virtual std::string GetFilename() = 0;
|
||||||
};
|
};
|
||||||
|
|
||||||
// An Implementation of file using std::ofstream.
|
// An Implementation of file using std::ofstream.
|
||||||
|
@ -50,20 +50,20 @@ class StreamFileWriter : public FileWriter {
|
||||||
public:
|
public:
|
||||||
~StreamFileWriter() override;
|
~StreamFileWriter() override;
|
||||||
|
|
||||||
StreamFileWriter(const string& filename);
|
StreamFileWriter(const std::string& filename);
|
||||||
|
|
||||||
bool Write(const char* data, size_t len) override;
|
bool Write(const char* data, size_t len) override;
|
||||||
bool WriteHeader(const char* data, size_t len) override;
|
bool WriteHeader(const char* data, size_t len) override;
|
||||||
bool Close() override;
|
bool Close() override;
|
||||||
string GetFilename() override;
|
std::string GetFilename() override;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
const string filename_;
|
const std::string filename_;
|
||||||
std::ofstream out_;
|
std::ofstream out_;
|
||||||
};
|
};
|
||||||
|
|
||||||
using FileWriterFactory =
|
using FileWriterFactory =
|
||||||
std::function<std::unique_ptr<FileWriter>(const string& filename)>;
|
std::function<std::unique_ptr<FileWriter>(const std::string& filename)>;
|
||||||
|
|
||||||
} // namespace io
|
} // namespace io
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
||||||
|
|
|
@ -27,7 +27,7 @@ namespace io {
|
||||||
std::unique_ptr<FrameIdFilteringPointsProcessor>
|
std::unique_ptr<FrameIdFilteringPointsProcessor>
|
||||||
FrameIdFilteringPointsProcessor::FromDictionary(
|
FrameIdFilteringPointsProcessor::FromDictionary(
|
||||||
common::LuaParameterDictionary* dictionary, PointsProcessor* next) {
|
common::LuaParameterDictionary* dictionary, PointsProcessor* next) {
|
||||||
std::vector<string> keep_frames, drop_frames;
|
std::vector<std::string> keep_frames, drop_frames;
|
||||||
if (dictionary->HasKey("keep_frames")) {
|
if (dictionary->HasKey("keep_frames")) {
|
||||||
keep_frames =
|
keep_frames =
|
||||||
dictionary->GetDictionary("keep_frames")->GetArrayValuesAsStrings();
|
dictionary->GetDictionary("keep_frames")->GetArrayValuesAsStrings();
|
||||||
|
@ -37,13 +37,15 @@ FrameIdFilteringPointsProcessor::FromDictionary(
|
||||||
dictionary->GetDictionary("drop_frames")->GetArrayValuesAsStrings();
|
dictionary->GetDictionary("drop_frames")->GetArrayValuesAsStrings();
|
||||||
}
|
}
|
||||||
return common::make_unique<FrameIdFilteringPointsProcessor>(
|
return common::make_unique<FrameIdFilteringPointsProcessor>(
|
||||||
std::unordered_set<string>(keep_frames.begin(), keep_frames.end()),
|
std::unordered_set<std::string>(keep_frames.begin(), keep_frames.end()),
|
||||||
std::unordered_set<string>(drop_frames.begin(), drop_frames.end()), next);
|
std::unordered_set<std::string>(drop_frames.begin(), drop_frames.end()),
|
||||||
|
next);
|
||||||
}
|
}
|
||||||
|
|
||||||
FrameIdFilteringPointsProcessor::FrameIdFilteringPointsProcessor(
|
FrameIdFilteringPointsProcessor::FrameIdFilteringPointsProcessor(
|
||||||
const std::unordered_set<string>& keep_frame_ids,
|
const std::unordered_set<std::string>& keep_frame_ids,
|
||||||
const std::unordered_set<string>& drop_frame_ids, PointsProcessor* next)
|
const std::unordered_set<std::string>& drop_frame_ids,
|
||||||
|
PointsProcessor* next)
|
||||||
: keep_frame_ids_(keep_frame_ids),
|
: keep_frame_ids_(keep_frame_ids),
|
||||||
drop_frame_ids_(drop_frame_ids),
|
drop_frame_ids_(drop_frame_ids),
|
||||||
next_(next) {
|
next_(next) {
|
||||||
|
|
|
@ -32,8 +32,9 @@ class FrameIdFilteringPointsProcessor : public PointsProcessor {
|
||||||
public:
|
public:
|
||||||
constexpr static const char* kConfigurationFileActionName = "frame_id_filter";
|
constexpr static const char* kConfigurationFileActionName = "frame_id_filter";
|
||||||
FrameIdFilteringPointsProcessor(
|
FrameIdFilteringPointsProcessor(
|
||||||
const std::unordered_set<string>& keep_frame_ids,
|
const std::unordered_set<std::string>& keep_frame_ids,
|
||||||
const std::unordered_set<string>& drop_frame_ids, PointsProcessor* next);
|
const std::unordered_set<std::string>& drop_frame_ids,
|
||||||
|
PointsProcessor* next);
|
||||||
static std::unique_ptr<FrameIdFilteringPointsProcessor> FromDictionary(
|
static std::unique_ptr<FrameIdFilteringPointsProcessor> FromDictionary(
|
||||||
common::LuaParameterDictionary* dictionary, PointsProcessor* next);
|
common::LuaParameterDictionary* dictionary, PointsProcessor* next);
|
||||||
~FrameIdFilteringPointsProcessor() override {}
|
~FrameIdFilteringPointsProcessor() override {}
|
||||||
|
@ -47,8 +48,8 @@ class FrameIdFilteringPointsProcessor : public PointsProcessor {
|
||||||
FlushResult Flush() override;
|
FlushResult Flush() override;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
const std::unordered_set<string> keep_frame_ids_;
|
const std::unordered_set<std::string> keep_frame_ids_;
|
||||||
const std::unordered_set<string> drop_frame_ids_;
|
const std::unordered_set<std::string> drop_frame_ids_;
|
||||||
PointsProcessor* const next_;
|
PointsProcessor* const next_;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -47,7 +47,7 @@ void HybridGridPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
|
||||||
PointsProcessor::FlushResult HybridGridPointsProcessor::Flush() {
|
PointsProcessor::FlushResult HybridGridPointsProcessor::Flush() {
|
||||||
const mapping_3d::proto::HybridGrid hybrid_grid_proto =
|
const mapping_3d::proto::HybridGrid hybrid_grid_proto =
|
||||||
hybrid_grid_.ToProto();
|
hybrid_grid_.ToProto();
|
||||||
string serialized;
|
std::string serialized;
|
||||||
hybrid_grid_proto.SerializeToString(&serialized);
|
hybrid_grid_proto.SerializeToString(&serialized);
|
||||||
file_writer_->Write(serialized.data(), serialized.size());
|
file_writer_->Write(serialized.data(), serialized.size());
|
||||||
CHECK(file_writer_->Close());
|
CHECK(file_writer_->Close());
|
||||||
|
|
|
@ -28,7 +28,7 @@ std::unique_ptr<IntensityToColorPointsProcessor>
|
||||||
IntensityToColorPointsProcessor::FromDictionary(
|
IntensityToColorPointsProcessor::FromDictionary(
|
||||||
common::LuaParameterDictionary* const dictionary,
|
common::LuaParameterDictionary* const dictionary,
|
||||||
PointsProcessor* const next) {
|
PointsProcessor* const next) {
|
||||||
const string frame_id =
|
const std::string frame_id =
|
||||||
dictionary->HasKey("frame_id") ? dictionary->GetString("frame_id") : "";
|
dictionary->HasKey("frame_id") ? dictionary->GetString("frame_id") : "";
|
||||||
const float min_intensity = dictionary->GetDouble("min_intensity");
|
const float min_intensity = dictionary->GetDouble("min_intensity");
|
||||||
const float max_intensity = dictionary->GetDouble("max_intensity");
|
const float max_intensity = dictionary->GetDouble("max_intensity");
|
||||||
|
@ -38,7 +38,7 @@ IntensityToColorPointsProcessor::FromDictionary(
|
||||||
|
|
||||||
IntensityToColorPointsProcessor::IntensityToColorPointsProcessor(
|
IntensityToColorPointsProcessor::IntensityToColorPointsProcessor(
|
||||||
const float min_intensity, const float max_intensity,
|
const float min_intensity, const float max_intensity,
|
||||||
const string& frame_id, PointsProcessor* const next)
|
const std::string& frame_id, PointsProcessor* const next)
|
||||||
: min_intensity_(min_intensity),
|
: min_intensity_(min_intensity),
|
||||||
max_intensity_(max_intensity),
|
max_intensity_(max_intensity),
|
||||||
frame_id_(frame_id),
|
frame_id_(frame_id),
|
||||||
|
|
|
@ -35,7 +35,7 @@ class IntensityToColorPointsProcessor : public PointsProcessor {
|
||||||
// with this value for each point that comes from the sensor with 'frame_id'.
|
// with this value for each point that comes from the sensor with 'frame_id'.
|
||||||
// If 'frame_id' is empty, this applies to all points.
|
// If 'frame_id' is empty, this applies to all points.
|
||||||
IntensityToColorPointsProcessor(float min_intensity, float max_intensity,
|
IntensityToColorPointsProcessor(float min_intensity, float max_intensity,
|
||||||
const string& frame_id,
|
const std::string& frame_id,
|
||||||
PointsProcessor* next);
|
PointsProcessor* next);
|
||||||
|
|
||||||
static std::unique_ptr<IntensityToColorPointsProcessor> FromDictionary(
|
static std::unique_ptr<IntensityToColorPointsProcessor> FromDictionary(
|
||||||
|
@ -54,7 +54,7 @@ class IntensityToColorPointsProcessor : public PointsProcessor {
|
||||||
private:
|
private:
|
||||||
const float min_intensity_;
|
const float min_intensity_;
|
||||||
const float max_intensity_;
|
const float max_intensity_;
|
||||||
const string frame_id_;
|
const std::string frame_id_;
|
||||||
PointsProcessor* const next_;
|
PointsProcessor* const next_;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -34,10 +34,10 @@ namespace {
|
||||||
// 'output_file'.
|
// 'output_file'.
|
||||||
void WriteBinaryPcdHeader(const bool has_color, const int64 num_points,
|
void WriteBinaryPcdHeader(const bool has_color, const int64 num_points,
|
||||||
FileWriter* const file_writer) {
|
FileWriter* const file_writer) {
|
||||||
string color_header_field = !has_color ? "" : " rgb";
|
std::string color_header_field = !has_color ? "" : " rgb";
|
||||||
string color_header_type = !has_color ? "" : " U";
|
std::string color_header_type = !has_color ? "" : " U";
|
||||||
string color_header_size = !has_color ? "" : " 4";
|
std::string color_header_size = !has_color ? "" : " 4";
|
||||||
string color_header_count = !has_color ? "" : " 1";
|
std::string color_header_count = !has_color ? "" : " 1";
|
||||||
|
|
||||||
std::ostringstream stream;
|
std::ostringstream stream;
|
||||||
stream << "# generated by Cartographer\n"
|
stream << "# generated by Cartographer\n"
|
||||||
|
@ -52,7 +52,7 @@ void WriteBinaryPcdHeader(const bool has_color, const int64 num_points,
|
||||||
<< "POINTS " << std::setw(15) << std::setfill('0') << num_points
|
<< "POINTS " << std::setw(15) << std::setfill('0') << num_points
|
||||||
<< "\n"
|
<< "\n"
|
||||||
<< "DATA binary\n";
|
<< "DATA binary\n";
|
||||||
const string out = stream.str();
|
const std::string out = stream.str();
|
||||||
file_writer->WriteHeader(out.data(), out.size());
|
file_writer->WriteHeader(out.data(), out.size());
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -34,7 +34,7 @@ namespace {
|
||||||
// 'output_file'.
|
// 'output_file'.
|
||||||
void WriteBinaryPlyHeader(const bool has_color, const int64 num_points,
|
void WriteBinaryPlyHeader(const bool has_color, const int64 num_points,
|
||||||
FileWriter* const file_writer) {
|
FileWriter* const file_writer) {
|
||||||
string color_header = !has_color ? ""
|
std::string color_header = !has_color ? ""
|
||||||
: "property uchar red\n"
|
: "property uchar red\n"
|
||||||
"property uchar green\n"
|
"property uchar green\n"
|
||||||
"property uchar blue\n";
|
"property uchar blue\n";
|
||||||
|
@ -48,7 +48,7 @@ void WriteBinaryPlyHeader(const bool has_color, const int64 num_points,
|
||||||
<< "property float y\n"
|
<< "property float y\n"
|
||||||
<< "property float z\n"
|
<< "property float z\n"
|
||||||
<< color_header << "end_header\n";
|
<< color_header << "end_header\n";
|
||||||
const string out = stream.str();
|
const std::string out = stream.str();
|
||||||
CHECK(file_writer->WriteHeader(out.data(), out.size()));
|
CHECK(file_writer->WriteHeader(out.data(), out.size()));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -46,7 +46,7 @@ struct PointsBatch {
|
||||||
|
|
||||||
// Sensor that generated this data's 'frame_id' or empty if this information
|
// Sensor that generated this data's 'frame_id' or empty if this information
|
||||||
// is unknown.
|
// is unknown.
|
||||||
string frame_id;
|
std::string frame_id;
|
||||||
|
|
||||||
// Trajectory ID that produced this point.
|
// Trajectory ID that produced this point.
|
||||||
int trajectory_id;
|
int trajectory_id;
|
||||||
|
|
|
@ -125,7 +125,7 @@ PointsProcessorPipelineBuilder::CreatePipeline(
|
||||||
|
|
||||||
// We construct the pipeline starting at the back.
|
// We construct the pipeline starting at the back.
|
||||||
for (auto it = configurations.rbegin(); it != configurations.rend(); it++) {
|
for (auto it = configurations.rbegin(); it != configurations.rend(); it++) {
|
||||||
const string action = (*it)->GetString("action");
|
const std::string action = (*it)->GetString("action");
|
||||||
auto factory_it = factories_.find(action);
|
auto factory_it = factories_.find(action);
|
||||||
CHECK(factory_it != factories_.end())
|
CHECK(factory_it != factories_.end())
|
||||||
<< "Unknown action '" << action
|
<< "Unknown action '" << action
|
||||||
|
|
|
@ -29,7 +29,7 @@ namespace {
|
||||||
TEST(PointsProcessorPipelineBuilderTest, RegisterBuiltInPointsProcessors) {
|
TEST(PointsProcessorPipelineBuilderTest, RegisterBuiltInPointsProcessors) {
|
||||||
PointsProcessorPipelineBuilder builder;
|
PointsProcessorPipelineBuilder builder;
|
||||||
FileWriterFactory dummy_factory =
|
FileWriterFactory dummy_factory =
|
||||||
[](const string& filename) -> std::unique_ptr<FileWriter> {
|
[](const std::string& filename) -> std::unique_ptr<FileWriter> {
|
||||||
return nullptr;
|
return nullptr;
|
||||||
};
|
};
|
||||||
RegisterBuiltInPointsProcessors({}, dummy_factory, &builder);
|
RegisterBuiltInPointsProcessors({}, dummy_factory, &builder);
|
||||||
|
|
|
@ -42,15 +42,15 @@ bool ReadSizeAsLittleEndian(std::istream* in, uint64* size) {
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
|
|
||||||
ProtoStreamWriter::ProtoStreamWriter(const string& filename)
|
ProtoStreamWriter::ProtoStreamWriter(const std::string& filename)
|
||||||
: out_(filename, std::ios::out | std::ios::binary) {
|
: out_(filename, std::ios::out | std::ios::binary) {
|
||||||
WriteSizeAsLittleEndian(kMagic, &out_);
|
WriteSizeAsLittleEndian(kMagic, &out_);
|
||||||
}
|
}
|
||||||
|
|
||||||
ProtoStreamWriter::~ProtoStreamWriter() {}
|
ProtoStreamWriter::~ProtoStreamWriter() {}
|
||||||
|
|
||||||
void ProtoStreamWriter::Write(const string& uncompressed_data) {
|
void ProtoStreamWriter::Write(const std::string& uncompressed_data) {
|
||||||
string compressed_data;
|
std::string compressed_data;
|
||||||
common::FastGzipString(uncompressed_data, &compressed_data);
|
common::FastGzipString(uncompressed_data, &compressed_data);
|
||||||
WriteSizeAsLittleEndian(compressed_data.size(), &out_);
|
WriteSizeAsLittleEndian(compressed_data.size(), &out_);
|
||||||
out_.write(compressed_data.data(), compressed_data.size());
|
out_.write(compressed_data.data(), compressed_data.size());
|
||||||
|
@ -61,7 +61,7 @@ bool ProtoStreamWriter::Close() {
|
||||||
return !out_.fail();
|
return !out_.fail();
|
||||||
}
|
}
|
||||||
|
|
||||||
ProtoStreamReader::ProtoStreamReader(const string& filename)
|
ProtoStreamReader::ProtoStreamReader(const std::string& filename)
|
||||||
: in_(filename, std::ios::in | std::ios::binary) {
|
: in_(filename, std::ios::in | std::ios::binary) {
|
||||||
uint64 magic;
|
uint64 magic;
|
||||||
if (!ReadSizeAsLittleEndian(&in_, &magic) || magic != kMagic) {
|
if (!ReadSizeAsLittleEndian(&in_, &magic) || magic != kMagic) {
|
||||||
|
@ -71,12 +71,12 @@ ProtoStreamReader::ProtoStreamReader(const string& filename)
|
||||||
|
|
||||||
ProtoStreamReader::~ProtoStreamReader() {}
|
ProtoStreamReader::~ProtoStreamReader() {}
|
||||||
|
|
||||||
bool ProtoStreamReader::Read(string* decompressed_data) {
|
bool ProtoStreamReader::Read(std::string* decompressed_data) {
|
||||||
uint64 compressed_size;
|
uint64 compressed_size;
|
||||||
if (!ReadSizeAsLittleEndian(&in_, &compressed_size)) {
|
if (!ReadSizeAsLittleEndian(&in_, &compressed_size)) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
string compressed_data(compressed_size, '\0');
|
std::string compressed_data(compressed_size, '\0');
|
||||||
if (!in_.read(&compressed_data.front(), compressed_size)) {
|
if (!in_.read(&compressed_data.front(), compressed_size)) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
|
@ -32,7 +32,7 @@ namespace io {
|
||||||
// compression performance? Should we use LZ4?
|
// compression performance? Should we use LZ4?
|
||||||
class ProtoStreamWriter {
|
class ProtoStreamWriter {
|
||||||
public:
|
public:
|
||||||
ProtoStreamWriter(const string& filename);
|
ProtoStreamWriter(const std::string& filename);
|
||||||
~ProtoStreamWriter();
|
~ProtoStreamWriter();
|
||||||
|
|
||||||
ProtoStreamWriter(const ProtoStreamWriter&) = delete;
|
ProtoStreamWriter(const ProtoStreamWriter&) = delete;
|
||||||
|
@ -41,7 +41,7 @@ class ProtoStreamWriter {
|
||||||
// Serializes, compressed and writes the 'proto' to the file.
|
// Serializes, compressed and writes the 'proto' to the file.
|
||||||
template <typename MessageType>
|
template <typename MessageType>
|
||||||
void WriteProto(const MessageType& proto) {
|
void WriteProto(const MessageType& proto) {
|
||||||
string uncompressed_data;
|
std::string uncompressed_data;
|
||||||
proto.SerializeToString(&uncompressed_data);
|
proto.SerializeToString(&uncompressed_data);
|
||||||
Write(uncompressed_data);
|
Write(uncompressed_data);
|
||||||
}
|
}
|
||||||
|
@ -50,7 +50,7 @@ class ProtoStreamWriter {
|
||||||
bool Close();
|
bool Close();
|
||||||
|
|
||||||
private:
|
private:
|
||||||
void Write(const string& uncompressed_data);
|
void Write(const std::string& uncompressed_data);
|
||||||
|
|
||||||
std::ofstream out_;
|
std::ofstream out_;
|
||||||
};
|
};
|
||||||
|
@ -58,7 +58,7 @@ class ProtoStreamWriter {
|
||||||
// A reader of the format produced by ProtoStreamWriter.
|
// A reader of the format produced by ProtoStreamWriter.
|
||||||
class ProtoStreamReader {
|
class ProtoStreamReader {
|
||||||
public:
|
public:
|
||||||
ProtoStreamReader(const string& filename);
|
ProtoStreamReader(const std::string& filename);
|
||||||
~ProtoStreamReader();
|
~ProtoStreamReader();
|
||||||
|
|
||||||
ProtoStreamReader(const ProtoStreamReader&) = delete;
|
ProtoStreamReader(const ProtoStreamReader&) = delete;
|
||||||
|
@ -66,7 +66,7 @@ class ProtoStreamReader {
|
||||||
|
|
||||||
template <typename MessageType>
|
template <typename MessageType>
|
||||||
bool ReadProto(MessageType* proto) {
|
bool ReadProto(MessageType* proto) {
|
||||||
string decompressed_data;
|
std::string decompressed_data;
|
||||||
return Read(&decompressed_data) &&
|
return Read(&decompressed_data) &&
|
||||||
proto->ParseFromString(decompressed_data);
|
proto->ParseFromString(decompressed_data);
|
||||||
}
|
}
|
||||||
|
@ -74,7 +74,7 @@ class ProtoStreamReader {
|
||||||
bool eof() const;
|
bool eof() const;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
bool Read(string* decompressed_data);
|
bool Read(std::string* decompressed_data);
|
||||||
|
|
||||||
std::ifstream in_;
|
std::ifstream in_;
|
||||||
};
|
};
|
||||||
|
|
|
@ -32,18 +32,18 @@ namespace {
|
||||||
class ProtoStreamTest : public ::testing::Test {
|
class ProtoStreamTest : public ::testing::Test {
|
||||||
protected:
|
protected:
|
||||||
void SetUp() override {
|
void SetUp() override {
|
||||||
const string tmpdir = P_tmpdir;
|
const std::string tmpdir = P_tmpdir;
|
||||||
test_directory_ = tmpdir + "/proto_stream_test_XXXXXX";
|
test_directory_ = tmpdir + "/proto_stream_test_XXXXXX";
|
||||||
ASSERT_NE(mkdtemp(&test_directory_[0]), nullptr) << strerror(errno);
|
ASSERT_NE(mkdtemp(&test_directory_[0]), nullptr) << strerror(errno);
|
||||||
}
|
}
|
||||||
|
|
||||||
void TearDown() override { remove(test_directory_.c_str()); }
|
void TearDown() override { remove(test_directory_.c_str()); }
|
||||||
|
|
||||||
string test_directory_;
|
std::string test_directory_;
|
||||||
};
|
};
|
||||||
|
|
||||||
TEST_F(ProtoStreamTest, WriteAndReadBack) {
|
TEST_F(ProtoStreamTest, WriteAndReadBack) {
|
||||||
const string test_file = test_directory_ + "/test_trajectory.pbstream";
|
const std::string test_file = test_directory_ + "/test_trajectory.pbstream";
|
||||||
{
|
{
|
||||||
ProtoStreamWriter writer(test_file);
|
ProtoStreamWriter writer(test_file);
|
||||||
for (int i = 0; i != 10; ++i) {
|
for (int i = 0; i != 10; ++i) {
|
||||||
|
|
|
@ -98,7 +98,8 @@ bool ContainedIn(const common::Time& time,
|
||||||
XRayPointsProcessor::XRayPointsProcessor(
|
XRayPointsProcessor::XRayPointsProcessor(
|
||||||
const double voxel_size, const transform::Rigid3f& transform,
|
const double voxel_size, const transform::Rigid3f& transform,
|
||||||
const std::vector<mapping::Floor>& floors,
|
const std::vector<mapping::Floor>& floors,
|
||||||
const DrawTrajectories& draw_trajectories, const string& output_filename,
|
const DrawTrajectories& draw_trajectories,
|
||||||
|
const std::string& output_filename,
|
||||||
const std::vector<mapping::proto::Trajectory>& trajectories,
|
const std::vector<mapping::proto::Trajectory>& trajectories,
|
||||||
FileWriterFactory file_writer_factory, PointsProcessor* const next)
|
FileWriterFactory file_writer_factory, PointsProcessor* const next)
|
||||||
: draw_trajectories_(draw_trajectories),
|
: draw_trajectories_(draw_trajectories),
|
||||||
|
|
|
@ -40,7 +40,8 @@ class XRayPointsProcessor : public PointsProcessor {
|
||||||
XRayPointsProcessor(
|
XRayPointsProcessor(
|
||||||
double voxel_size, const transform::Rigid3f& transform,
|
double voxel_size, const transform::Rigid3f& transform,
|
||||||
const std::vector<mapping::Floor>& floors,
|
const std::vector<mapping::Floor>& floors,
|
||||||
const DrawTrajectories& draw_trajectories, const string& output_filename,
|
const DrawTrajectories& draw_trajectories,
|
||||||
|
const std::string& output_filename,
|
||||||
const std::vector<mapping::proto::Trajectory>& trajectories,
|
const std::vector<mapping::proto::Trajectory>& trajectories,
|
||||||
FileWriterFactory file_writer_factory, PointsProcessor* next);
|
FileWriterFactory file_writer_factory, PointsProcessor* next);
|
||||||
|
|
||||||
|
@ -81,7 +82,7 @@ class XRayPointsProcessor : public PointsProcessor {
|
||||||
// If empty, we do not separate into floors.
|
// If empty, we do not separate into floors.
|
||||||
std::vector<mapping::Floor> floors_;
|
std::vector<mapping::Floor> floors_;
|
||||||
|
|
||||||
const string output_filename_;
|
const std::string output_filename_;
|
||||||
const transform::Rigid3f transform_;
|
const transform::Rigid3f transform_;
|
||||||
|
|
||||||
// Only has one entry if we do not separate into floors.
|
// Only has one entry if we do not separate into floors.
|
||||||
|
|
|
@ -15,7 +15,7 @@ void WriteXyzPoint(const Eigen::Vector3f& point,
|
||||||
std::ostringstream stream;
|
std::ostringstream stream;
|
||||||
stream << std::setprecision(6);
|
stream << std::setprecision(6);
|
||||||
stream << point.x() << " " << point.y() << " " << point.z() << "\n";
|
stream << point.x() << " " << point.y() << " " << point.z() << "\n";
|
||||||
const string out = stream.str();
|
const std::string out = stream.str();
|
||||||
CHECK(file_writer->Write(out.data(), out.size()));
|
CHECK(file_writer->Write(out.data(), out.size()));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -30,7 +30,7 @@ constexpr double kSensorDataRatesLoggingPeriodSeconds = 15.;
|
||||||
|
|
||||||
CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
|
CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
|
||||||
sensor::Collator* const sensor_collator, const int trajectory_id,
|
sensor::Collator* const sensor_collator, const int trajectory_id,
|
||||||
const std::unordered_set<string>& expected_sensor_ids,
|
const std::unordered_set<std::string>& expected_sensor_ids,
|
||||||
std::unique_ptr<GlobalTrajectoryBuilderInterface>
|
std::unique_ptr<GlobalTrajectoryBuilderInterface>
|
||||||
wrapped_trajectory_builder)
|
wrapped_trajectory_builder)
|
||||||
: sensor_collator_(sensor_collator),
|
: sensor_collator_(sensor_collator),
|
||||||
|
@ -39,7 +39,7 @@ CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
|
||||||
last_logging_time_(std::chrono::steady_clock::now()) {
|
last_logging_time_(std::chrono::steady_clock::now()) {
|
||||||
sensor_collator_->AddTrajectory(
|
sensor_collator_->AddTrajectory(
|
||||||
trajectory_id, expected_sensor_ids,
|
trajectory_id, expected_sensor_ids,
|
||||||
[this](const string& sensor_id, std::unique_ptr<sensor::Data> data) {
|
[this](const std::string& sensor_id, std::unique_ptr<sensor::Data> data) {
|
||||||
HandleCollatedSensorData(sensor_id, std::move(data));
|
HandleCollatedSensorData(sensor_id, std::move(data));
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
|
@ -51,12 +51,12 @@ const PoseEstimate& CollatedTrajectoryBuilder::pose_estimate() const {
|
||||||
}
|
}
|
||||||
|
|
||||||
void CollatedTrajectoryBuilder::AddSensorData(
|
void CollatedTrajectoryBuilder::AddSensorData(
|
||||||
const string& sensor_id, std::unique_ptr<sensor::Data> data) {
|
const std::string& sensor_id, std::unique_ptr<sensor::Data> data) {
|
||||||
sensor_collator_->AddSensorData(trajectory_id_, sensor_id, std::move(data));
|
sensor_collator_->AddSensorData(trajectory_id_, sensor_id, std::move(data));
|
||||||
}
|
}
|
||||||
|
|
||||||
void CollatedTrajectoryBuilder::HandleCollatedSensorData(
|
void CollatedTrajectoryBuilder::HandleCollatedSensorData(
|
||||||
const string& sensor_id, std::unique_ptr<sensor::Data> data) {
|
const std::string& sensor_id, std::unique_ptr<sensor::Data> data) {
|
||||||
auto it = rate_timers_.find(sensor_id);
|
auto it = rate_timers_.find(sensor_id);
|
||||||
if (it == rate_timers_.end()) {
|
if (it == rate_timers_.end()) {
|
||||||
it = rate_timers_
|
it = rate_timers_
|
||||||
|
|
|
@ -40,7 +40,7 @@ class CollatedTrajectoryBuilder : public TrajectoryBuilder {
|
||||||
public:
|
public:
|
||||||
CollatedTrajectoryBuilder(
|
CollatedTrajectoryBuilder(
|
||||||
sensor::Collator* sensor_collator, int trajectory_id,
|
sensor::Collator* sensor_collator, int trajectory_id,
|
||||||
const std::unordered_set<string>& expected_sensor_ids,
|
const std::unordered_set<std::string>& expected_sensor_ids,
|
||||||
std::unique_ptr<GlobalTrajectoryBuilderInterface>
|
std::unique_ptr<GlobalTrajectoryBuilderInterface>
|
||||||
wrapped_trajectory_builder);
|
wrapped_trajectory_builder);
|
||||||
~CollatedTrajectoryBuilder() override;
|
~CollatedTrajectoryBuilder() override;
|
||||||
|
@ -51,11 +51,11 @@ class CollatedTrajectoryBuilder : public TrajectoryBuilder {
|
||||||
|
|
||||||
const PoseEstimate& pose_estimate() const override;
|
const PoseEstimate& pose_estimate() const override;
|
||||||
|
|
||||||
void AddSensorData(const string& sensor_id,
|
void AddSensorData(const std::string& sensor_id,
|
||||||
std::unique_ptr<sensor::Data> data) override;
|
std::unique_ptr<sensor::Data> data) override;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
void HandleCollatedSensorData(const string& sensor_id,
|
void HandleCollatedSensorData(const std::string& sensor_id,
|
||||||
std::unique_ptr<sensor::Data> data);
|
std::unique_ptr<sensor::Data> data);
|
||||||
|
|
||||||
sensor::Collator* const sensor_collator_;
|
sensor::Collator* const sensor_collator_;
|
||||||
|
@ -64,7 +64,7 @@ class CollatedTrajectoryBuilder : public TrajectoryBuilder {
|
||||||
|
|
||||||
// Time at which we last logged the rates of incoming sensor data.
|
// Time at which we last logged the rates of incoming sensor data.
|
||||||
std::chrono::steady_clock::time_point last_logging_time_;
|
std::chrono::steady_clock::time_point last_logging_time_;
|
||||||
std::map<string, common::RateTimer<>> rate_timers_;
|
std::map<std::string, common::RateTimer<>> rate_timers_;
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace mapping
|
} // namespace mapping
|
||||||
|
|
|
@ -74,7 +74,7 @@ MapBuilder::MapBuilder(
|
||||||
MapBuilder::~MapBuilder() {}
|
MapBuilder::~MapBuilder() {}
|
||||||
|
|
||||||
int MapBuilder::AddTrajectoryBuilder(
|
int MapBuilder::AddTrajectoryBuilder(
|
||||||
const std::unordered_set<string>& expected_sensor_ids,
|
const std::unordered_set<std::string>& expected_sensor_ids,
|
||||||
const proto::TrajectoryBuilderOptions& trajectory_options) {
|
const proto::TrajectoryBuilderOptions& trajectory_options) {
|
||||||
const int trajectory_id = trajectory_builders_.size();
|
const int trajectory_id = trajectory_builders_.size();
|
||||||
if (options_.use_trajectory_builder_3d()) {
|
if (options_.use_trajectory_builder_3d()) {
|
||||||
|
@ -138,7 +138,8 @@ int MapBuilder::GetBlockingTrajectoryId() const {
|
||||||
return sensor_collator_.GetBlockingTrajectoryId();
|
return sensor_collator_.GetBlockingTrajectoryId();
|
||||||
}
|
}
|
||||||
|
|
||||||
string MapBuilder::SubmapToProto(const mapping::SubmapId& submap_id,
|
std::string MapBuilder::SubmapToProto(
|
||||||
|
const mapping::SubmapId& submap_id,
|
||||||
proto::SubmapQuery::Response* const response) {
|
proto::SubmapQuery::Response* const response) {
|
||||||
if (submap_id.trajectory_id < 0 ||
|
if (submap_id.trajectory_id < 0 ||
|
||||||
submap_id.trajectory_id >= num_trajectory_builders()) {
|
submap_id.trajectory_id >= num_trajectory_builders()) {
|
||||||
|
|
|
@ -61,7 +61,7 @@ class MapBuilder {
|
||||||
|
|
||||||
// Creates a new trajectory builder and returns its index.
|
// Creates a new trajectory builder and returns its index.
|
||||||
int AddTrajectoryBuilder(
|
int AddTrajectoryBuilder(
|
||||||
const std::unordered_set<string>& expected_sensor_ids,
|
const std::unordered_set<std::string>& expected_sensor_ids,
|
||||||
const proto::TrajectoryBuilderOptions& trajectory_options);
|
const proto::TrajectoryBuilderOptions& trajectory_options);
|
||||||
|
|
||||||
// Creates a new trajectory and returns its index. Querying the trajectory
|
// Creates a new trajectory and returns its index. Querying the trajectory
|
||||||
|
@ -84,7 +84,7 @@ class MapBuilder {
|
||||||
|
|
||||||
// Fills the SubmapQuery::Response corresponding to 'submap_id'. Returns an
|
// Fills the SubmapQuery::Response corresponding to 'submap_id'. Returns an
|
||||||
// error string on failure, or an empty string on success.
|
// error string on failure, or an empty string on success.
|
||||||
string SubmapToProto(const SubmapId& submap_id,
|
std::string SubmapToProto(const SubmapId& submap_id,
|
||||||
proto::SubmapQuery::Response* response);
|
proto::SubmapQuery::Response* response);
|
||||||
|
|
||||||
// Serializes the current state to a proto stream.
|
// Serializes the current state to a proto stream.
|
||||||
|
|
|
@ -50,10 +50,10 @@ class TrajectoryBuilder {
|
||||||
|
|
||||||
virtual const PoseEstimate& pose_estimate() const = 0;
|
virtual const PoseEstimate& pose_estimate() const = 0;
|
||||||
|
|
||||||
virtual void AddSensorData(const string& sensor_id,
|
virtual void AddSensorData(const std::string& sensor_id,
|
||||||
std::unique_ptr<sensor::Data> data) = 0;
|
std::unique_ptr<sensor::Data> data) = 0;
|
||||||
|
|
||||||
void AddRangefinderData(const string& sensor_id, common::Time time,
|
void AddRangefinderData(const std::string& sensor_id, common::Time time,
|
||||||
const Eigen::Vector3f& origin,
|
const Eigen::Vector3f& origin,
|
||||||
const sensor::TimedPointCloud& ranges) {
|
const sensor::TimedPointCloud& ranges) {
|
||||||
AddSensorData(sensor_id,
|
AddSensorData(sensor_id,
|
||||||
|
@ -61,20 +61,20 @@ class TrajectoryBuilder {
|
||||||
time, origin, ranges));
|
time, origin, ranges));
|
||||||
}
|
}
|
||||||
|
|
||||||
void AddImuData(const string& sensor_id, common::Time time,
|
void AddImuData(const std::string& sensor_id, common::Time time,
|
||||||
const Eigen::Vector3d& linear_acceleration,
|
const Eigen::Vector3d& linear_acceleration,
|
||||||
const Eigen::Vector3d& angular_velocity) {
|
const Eigen::Vector3d& angular_velocity) {
|
||||||
AddSensorData(sensor_id, sensor::MakeDispatchable(sensor::ImuData{
|
AddSensorData(sensor_id, sensor::MakeDispatchable(sensor::ImuData{
|
||||||
time, linear_acceleration, angular_velocity}));
|
time, linear_acceleration, angular_velocity}));
|
||||||
}
|
}
|
||||||
|
|
||||||
void AddOdometerData(const string& sensor_id, common::Time time,
|
void AddOdometerData(const std::string& sensor_id, common::Time time,
|
||||||
const transform::Rigid3d& odometer_pose) {
|
const transform::Rigid3d& odometer_pose) {
|
||||||
AddSensorData(sensor_id, sensor::MakeDispatchable(
|
AddSensorData(sensor_id, sensor::MakeDispatchable(
|
||||||
sensor::OdometryData{time, odometer_pose}));
|
sensor::OdometryData{time, odometer_pose}));
|
||||||
}
|
}
|
||||||
|
|
||||||
void AddFixedFramePoseData(const string& sensor_id, common::Time time,
|
void AddFixedFramePoseData(const std::string& sensor_id, common::Time time,
|
||||||
const transform::Rigid3d& fixed_frame_pose) {
|
const transform::Rigid3d& fixed_frame_pose) {
|
||||||
AddSensorData(sensor_id,
|
AddSensorData(sensor_id,
|
||||||
sensor::MakeDispatchable(
|
sensor::MakeDispatchable(
|
||||||
|
|
|
@ -91,7 +91,7 @@ void Submap::ToResponseProto(
|
||||||
CellLimits limits;
|
CellLimits limits;
|
||||||
probability_grid_.ComputeCroppedLimits(&offset, &limits);
|
probability_grid_.ComputeCroppedLimits(&offset, &limits);
|
||||||
|
|
||||||
string cells;
|
std::string cells;
|
||||||
for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(limits)) {
|
for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(limits)) {
|
||||||
if (probability_grid_.IsKnown(xy_index + offset)) {
|
if (probability_grid_.IsKnown(xy_index + offset)) {
|
||||||
// We would like to add 'delta' but this is not possible using a value and
|
// We would like to add 'delta' but this is not possible using a value and
|
||||||
|
|
|
@ -40,7 +40,8 @@ proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions(
|
||||||
common::LuaParameterDictionary* const parameter_dictionary) {
|
common::LuaParameterDictionary* const parameter_dictionary) {
|
||||||
proto::CeresScanMatcherOptions options;
|
proto::CeresScanMatcherOptions options;
|
||||||
for (int i = 0;; ++i) {
|
for (int i = 0;; ++i) {
|
||||||
const string lua_identifier = "occupied_space_weight_" + std::to_string(i);
|
const std::string lua_identifier =
|
||||||
|
"occupied_space_weight_" + std::to_string(i);
|
||||||
if (!parameter_dictionary->HasKey(lua_identifier)) {
|
if (!parameter_dictionary->HasKey(lua_identifier)) {
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
|
@ -113,9 +113,9 @@ std::vector<Eigen::Array4i> ExtractVoxelData(
|
||||||
|
|
||||||
// Builds texture data containing interleaved value and alpha for the
|
// Builds texture data containing interleaved value and alpha for the
|
||||||
// visualization from 'accumulated_pixel_data'.
|
// visualization from 'accumulated_pixel_data'.
|
||||||
string ComputePixelValues(
|
std::string ComputePixelValues(
|
||||||
const std::vector<PixelData>& accumulated_pixel_data) {
|
const std::vector<PixelData>& accumulated_pixel_data) {
|
||||||
string cell_data;
|
std::string cell_data;
|
||||||
cell_data.reserve(2 * accumulated_pixel_data.size());
|
cell_data.reserve(2 * accumulated_pixel_data.size());
|
||||||
constexpr float kMinZDifference = 3.f;
|
constexpr float kMinZDifference = 3.f;
|
||||||
constexpr float kFreeSpaceWeight = 0.15f;
|
constexpr float kFreeSpaceWeight = 0.15f;
|
||||||
|
@ -168,7 +168,7 @@ void AddToTextureProto(
|
||||||
|
|
||||||
const std::vector<PixelData> accumulated_pixel_data = AccumulatePixelData(
|
const std::vector<PixelData> accumulated_pixel_data = AccumulatePixelData(
|
||||||
width, height, min_index, max_index, voxel_indices_and_probabilities);
|
width, height, min_index, max_index, voxel_indices_and_probabilities);
|
||||||
const string cell_data = ComputePixelValues(accumulated_pixel_data);
|
const std::string cell_data = ComputePixelValues(accumulated_pixel_data);
|
||||||
|
|
||||||
common::FastGzipString(cell_data, texture->mutable_cells());
|
common::FastGzipString(cell_data, texture->mutable_cells());
|
||||||
*texture->mutable_slice_pose() = transform::ToProto(
|
*texture->mutable_slice_pose() = transform::ToProto(
|
||||||
|
|
|
@ -21,7 +21,7 @@ namespace sensor {
|
||||||
|
|
||||||
void Collator::AddTrajectory(
|
void Collator::AddTrajectory(
|
||||||
const int trajectory_id,
|
const int trajectory_id,
|
||||||
const std::unordered_set<string>& expected_sensor_ids,
|
const std::unordered_set<std::string>& expected_sensor_ids,
|
||||||
const Callback& callback) {
|
const Callback& callback) {
|
||||||
for (const auto& sensor_id : expected_sensor_ids) {
|
for (const auto& sensor_id : expected_sensor_ids) {
|
||||||
const auto queue_key = QueueKey{trajectory_id, sensor_id};
|
const auto queue_key = QueueKey{trajectory_id, sensor_id};
|
||||||
|
@ -39,7 +39,8 @@ void Collator::FinishTrajectory(const int trajectory_id) {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void Collator::AddSensorData(const int trajectory_id, const string& sensor_id,
|
void Collator::AddSensorData(const int trajectory_id,
|
||||||
|
const std::string& sensor_id,
|
||||||
std::unique_ptr<Data> data) {
|
std::unique_ptr<Data> data) {
|
||||||
queue_.Add(QueueKey{trajectory_id, sensor_id}, std::move(data));
|
queue_.Add(QueueKey{trajectory_id, sensor_id}, std::move(data));
|
||||||
}
|
}
|
||||||
|
|
|
@ -31,7 +31,8 @@ namespace sensor {
|
||||||
|
|
||||||
class Collator {
|
class Collator {
|
||||||
public:
|
public:
|
||||||
using Callback = std::function<void(const string&, std::unique_ptr<Data>)>;
|
using Callback =
|
||||||
|
std::function<void(const std::string&, std::unique_ptr<Data>)>;
|
||||||
|
|
||||||
Collator() {}
|
Collator() {}
|
||||||
|
|
||||||
|
@ -41,7 +42,7 @@ class Collator {
|
||||||
// Adds a trajectory to produce sorted sensor output for. Calls 'callback'
|
// Adds a trajectory to produce sorted sensor output for. Calls 'callback'
|
||||||
// for each collated sensor data.
|
// for each collated sensor data.
|
||||||
void AddTrajectory(int trajectory_id,
|
void AddTrajectory(int trajectory_id,
|
||||||
const std::unordered_set<string>& expected_sensor_ids,
|
const std::unordered_set<std::string>& expected_sensor_ids,
|
||||||
const Callback& callback);
|
const Callback& callback);
|
||||||
|
|
||||||
// Marks 'trajectory_id' as finished.
|
// Marks 'trajectory_id' as finished.
|
||||||
|
@ -50,7 +51,7 @@ class Collator {
|
||||||
// Adds 'data' for 'trajectory_id' to be collated. 'data' must contain valid
|
// Adds 'data' for 'trajectory_id' to be collated. 'data' must contain valid
|
||||||
// sensor data. Sensor packets with matching 'sensor_id' must be added in time
|
// sensor data. Sensor packets with matching 'sensor_id' must be added in time
|
||||||
// order.
|
// order.
|
||||||
void AddSensorData(int trajectory_id, const string& sensor_id,
|
void AddSensorData(int trajectory_id, const std::string& sensor_id,
|
||||||
std::unique_ptr<Data> data);
|
std::unique_ptr<Data> data);
|
||||||
|
|
||||||
// Dispatches all queued sensor packets. May only be called once.
|
// Dispatches all queued sensor packets. May only be called once.
|
||||||
|
|
|
@ -30,7 +30,7 @@ namespace sensor {
|
||||||
namespace {
|
namespace {
|
||||||
|
|
||||||
TEST(Collator, Ordering) {
|
TEST(Collator, Ordering) {
|
||||||
const std::array<string, 4> kSensorId = {
|
const std::array<std::string, 4> kSensorId = {
|
||||||
{"horizontal_rangefinder", "vertical_rangefinder", "imu", "odometry"}};
|
{"horizontal_rangefinder", "vertical_rangefinder", "imu", "odometry"}};
|
||||||
DispatchableRangefinderData zero(common::FromUniversal(0),
|
DispatchableRangefinderData zero(common::FromUniversal(0),
|
||||||
Eigen::Vector3f::Zero(), {});
|
Eigen::Vector3f::Zero(), {});
|
||||||
|
@ -46,11 +46,11 @@ TEST(Collator, Ordering) {
|
||||||
OdometryData sixth{common::FromUniversal(600),
|
OdometryData sixth{common::FromUniversal(600),
|
||||||
transform::Rigid3d::Identity()};
|
transform::Rigid3d::Identity()};
|
||||||
|
|
||||||
std::vector<std::pair<string, common::Time>> received;
|
std::vector<std::pair<std::string, common::Time>> received;
|
||||||
Collator collator;
|
Collator collator;
|
||||||
collator.AddTrajectory(
|
collator.AddTrajectory(
|
||||||
0, std::unordered_set<string>(kSensorId.begin(), kSensorId.end()),
|
0, std::unordered_set<std::string>(kSensorId.begin(), kSensorId.end()),
|
||||||
[&received](const string& sensor_id, std::unique_ptr<Data> data) {
|
[&received](const std::string& sensor_id, std::unique_ptr<Data> data) {
|
||||||
received.push_back(std::make_pair(sensor_id, data->GetTime()));
|
received.push_back(std::make_pair(sensor_id, data->GetTime()));
|
||||||
});
|
});
|
||||||
|
|
||||||
|
|
|
@ -41,7 +41,7 @@ constexpr float kPrecision = 0.001f;
|
||||||
|
|
||||||
// Matcher for 3-d vectors w.r.t. to the target precision.
|
// Matcher for 3-d vectors w.r.t. to the target precision.
|
||||||
MATCHER_P(ApproximatelyEquals, expected,
|
MATCHER_P(ApproximatelyEquals, expected,
|
||||||
string("is equal to ") + PrintToString(expected)) {
|
std::string("is equal to ") + PrintToString(expected)) {
|
||||||
return (arg - expected).isZero(kPrecision);
|
return (arg - expected).isZero(kPrecision);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -30,7 +30,7 @@ namespace cartographer {
|
||||||
namespace sensor {
|
namespace sensor {
|
||||||
|
|
||||||
struct Landmark {
|
struct Landmark {
|
||||||
string id;
|
std::string id;
|
||||||
transform::Rigid3d transform;
|
transform::Rigid3d transform;
|
||||||
double translation_weight;
|
double translation_weight;
|
||||||
double rotation_weight;
|
double rotation_weight;
|
||||||
|
|
|
@ -33,7 +33,7 @@ namespace sensor {
|
||||||
|
|
||||||
struct QueueKey {
|
struct QueueKey {
|
||||||
int trajectory_id;
|
int trajectory_id;
|
||||||
string sensor_id;
|
std::string sensor_id;
|
||||||
|
|
||||||
bool operator<(const QueueKey& other) const {
|
bool operator<(const QueueKey& other) const {
|
||||||
return std::forward_as_tuple(trajectory_id, sensor_id) <
|
return std::forward_as_tuple(trajectory_id, sensor_id) <
|
||||||
|
|
|
@ -76,8 +76,8 @@ class Rigid2 {
|
||||||
return Rigid2(translation, rotation);
|
return Rigid2(translation, rotation);
|
||||||
}
|
}
|
||||||
|
|
||||||
string DebugString() const {
|
std::string DebugString() const {
|
||||||
string out;
|
std::string out;
|
||||||
out.append("{ t: [");
|
out.append("{ t: [");
|
||||||
out.append(std::to_string(translation().x()));
|
out.append(std::to_string(translation().x()));
|
||||||
out.append(", ");
|
out.append(", ");
|
||||||
|
@ -161,8 +161,8 @@ class Rigid3 {
|
||||||
return Rigid3(translation, rotation);
|
return Rigid3(translation, rotation);
|
||||||
}
|
}
|
||||||
|
|
||||||
string DebugString() const {
|
std::string DebugString() const {
|
||||||
string out;
|
std::string out;
|
||||||
out.append("{ t: [");
|
out.append("{ t: [");
|
||||||
out.append(std::to_string(translation().x()));
|
out.append(std::to_string(translation().x()));
|
||||||
out.append(", ");
|
out.append(", ");
|
||||||
|
|
|
@ -40,7 +40,7 @@ Eigen::Transform<T, 3, Eigen::Affine> ToEigen(const Rigid3<T>& rigid3) {
|
||||||
}
|
}
|
||||||
|
|
||||||
MATCHER_P2(IsNearly, rigid, epsilon,
|
MATCHER_P2(IsNearly, rigid, epsilon,
|
||||||
string(string(negation ? "isn't" : "is", " nearly ") +
|
std::string(std::string(negation ? "isn't" : "is", " nearly ") +
|
||||||
rigid.DebugString())) {
|
rigid.DebugString())) {
|
||||||
return ToEigen(arg).isApprox(ToEigen(rigid), epsilon);
|
return ToEigen(arg).isApprox(ToEigen(rigid), epsilon);
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue