00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "cartographer/io/probability_grid_points_processor.h"
00018
00019 #include <string>
00020
00021 #include "cartographer/common/lua_parameter_dictionary.h"
00022 #include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
00023 #include "cartographer/common/port.h"
00024 #include "cartographer/io/fake_file_writer.h"
00025 #include "cartographer/io/points_processor_pipeline_builder.h"
00026 #include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h"
00027 #include "cartographer/mapping/value_conversion_tables.h"
00028 #include "gmock/gmock.h"
00029 #include "gtest/gtest.h"
00030
00031 namespace cartographer {
00032 namespace io {
00033 namespace {
00034
00035 std::unique_ptr<PointsBatch> CreatePointsBatch() {
00036 auto points_batch = ::absl::make_unique<PointsBatch>();
00037 points_batch->origin = Eigen::Vector3f(0, 0, 0);
00038 points_batch->points.push_back({Eigen::Vector3f{0.0f, 0.0f, 0.0f}});
00039 points_batch->points.push_back({Eigen::Vector3f{0.0f, 1.0f, 2.0f}});
00040 points_batch->points.push_back({Eigen::Vector3f{1.0f, 2.0f, 4.0f}});
00041 points_batch->points.push_back({Eigen::Vector3f{0.0f, 3.0f, 5.0f}});
00042 points_batch->points.push_back({Eigen::Vector3f{3.0f, 0.0f, 6.0f}});
00043 return points_batch;
00044 }
00045
00046 ::cartographer::io::FileWriterFactory CreateFakeFileWriterFactory(
00047 const std::string& expected_filename,
00048 std::shared_ptr<std::vector<char>> fake_file_writer_output) {
00049 return [&fake_file_writer_output,
00050 &expected_filename](const std::string& full_filename) {
00051 EXPECT_EQ(expected_filename, full_filename);
00052 return ::absl::make_unique<::cartographer::io::FakeFileWriter>(
00053 full_filename, fake_file_writer_output);
00054 };
00055 }
00056
00057 std::vector<std::unique_ptr<::cartographer::io::PointsProcessor>>
00058 CreatePipelineFromDictionary(
00059 common::LuaParameterDictionary* const pipeline_dictionary,
00060 const std::vector<mapping::proto::Trajectory>& trajectories,
00061 ::cartographer::io::FileWriterFactory file_writer_factory) {
00062 auto builder =
00063 ::absl::make_unique<::cartographer::io::PointsProcessorPipelineBuilder>();
00064 builder->Register(
00065 ProbabilityGridPointsProcessor::kConfigurationFileActionName,
00066 [&trajectories, file_writer_factory](
00067 common::LuaParameterDictionary* const dictionary,
00068 PointsProcessor* const next) -> std::unique_ptr<PointsProcessor> {
00069 return ProbabilityGridPointsProcessor::FromDictionary(
00070 trajectories, file_writer_factory, dictionary, next);
00071 });
00072
00073 return builder->CreatePipeline(pipeline_dictionary);
00074 }
00075
00076 std::vector<char> CreateExpectedProbabilityGrid(
00077 std::unique_ptr<PointsBatch> points_batch,
00078 common::LuaParameterDictionary* const probability_grid_options) {
00079 ::cartographer::mapping::ProbabilityGridRangeDataInserter2D
00080 range_data_inserter(
00081 cartographer::mapping::
00082 CreateProbabilityGridRangeDataInserterOptions2D(
00083 probability_grid_options->GetDictionary("range_data_inserter")
00084 .get()));
00085 mapping::ValueConversionTables conversion_tables;
00086 auto probability_grid = CreateProbabilityGrid(
00087 probability_grid_options->GetDouble("resolution"), &conversion_tables);
00088 range_data_inserter.Insert({points_batch->origin, points_batch->points, {}},
00089 &probability_grid);
00090
00091 std::vector<char> probability_grid_proto(
00092 probability_grid.ToProto().ByteSize());
00093 probability_grid.ToProto().SerializePartialToArray(
00094 probability_grid_proto.data(), probability_grid_proto.size());
00095 return probability_grid_proto;
00096 }
00097
00098 std::unique_ptr<common::LuaParameterDictionary> CreateParameterDictionary() {
00099 auto parameter_dictionary =
00100 cartographer::common::LuaParameterDictionary::NonReferenceCounted(
00101 R"text(
00102 pipeline = {
00103 {
00104 action = "write_probability_grid",
00105 resolution = 0.05,
00106 range_data_inserter = {
00107 insert_free_space = true,
00108 hit_probability = 0.55,
00109 miss_probability = 0.49,
00110 },
00111 draw_trajectories = false,
00112 output_type = "pb",
00113 filename = "map"
00114 }
00115 }
00116 return pipeline
00117 )text",
00118 absl::make_unique<cartographer::common::DummyFileResolver>());
00119 return parameter_dictionary;
00120 }
00121
00122 class ProbabilityGridPointsProcessorTest : public ::testing::Test {
00123 protected:
00124 ProbabilityGridPointsProcessorTest()
00125 : pipeline_dictionary_(CreateParameterDictionary()) {}
00126
00127 void Run(const std::string& expected_filename) {
00128 const auto pipeline = CreatePipelineFromDictionary(
00129 pipeline_dictionary_.get(), dummy_trajectories_,
00130 CreateFakeFileWriterFactory(expected_filename,
00131 fake_file_writer_output_));
00132 EXPECT_TRUE(pipeline.size() > 0);
00133
00134 do {
00135 pipeline.back()->Process(CreatePointsBatch());
00136 } while (pipeline.back()->Flush() ==
00137 cartographer::io::PointsProcessor::FlushResult::kRestartStream);
00138 }
00139
00140 std::shared_ptr<std::vector<char>> fake_file_writer_output_ =
00141 std::make_shared<std::vector<char>>();
00142 std::unique_ptr<cartographer::common::LuaParameterDictionary>
00143 pipeline_dictionary_;
00144 const std::vector<mapping::proto::Trajectory> dummy_trajectories_;
00145 };
00146
00147 TEST_F(ProbabilityGridPointsProcessorTest, WriteProto) {
00148 const auto expected_prob_grid_proto = CreateExpectedProbabilityGrid(
00149 CreatePointsBatch(),
00150 pipeline_dictionary_->GetArrayValuesAsDictionaries().front().get());
00151 Run("map.pb");
00152 EXPECT_THAT(*fake_file_writer_output_,
00153 ::testing::ContainerEq(expected_prob_grid_proto));
00154 }
00155
00156 }
00157 }
00158 }