diff --git a/cartographer/mapping_3d/hybrid_grid.h b/cartographer/mapping_3d/hybrid_grid.h index 91e9ada..12a55bc 100644 --- a/cartographer/mapping_3d/hybrid_grid.h +++ b/cartographer/mapping_3d/hybrid_grid.h @@ -26,7 +26,9 @@ #include "cartographer/common/make_unique.h" #include "cartographer/common/math.h" #include "cartographer/common/port.h" +#include "cartographer/mapping_3d/proto/hybrid_grid.pb.h" #include "cartographer/mapping/probability_values.h" +#include "cartographer/transform/transform.h" #include "glog/logging.h" namespace cartographer { @@ -468,6 +470,20 @@ class HybridGrid : public HybridGridBase { HybridGrid(const float resolution, const Eigen::Vector3f& origin) : HybridGridBase(resolution, origin) {} + HybridGrid(const proto::HybridGrid& proto) + : HybridGrid(proto.resolution(), transform::ToEigen(proto.origin())) { + CHECK_EQ(proto.values_size(), proto.x_indices_size()); + CHECK_EQ(proto.values_size(), proto.y_indices_size()); + CHECK_EQ(proto.values_size(), proto.z_indices_size()); + + for (int i = 0; i < proto.values_size(); ++i) { + // SetProbability does some error checking for us. + SetProbability(Eigen::Vector3i(proto.x_indices(i), proto.y_indices(i), + proto.z_indices(i)), + mapping::ValueToProbability(proto.values(i))); + } + } + // Sets the probability of the cell at 'index' to the given 'probability'. void SetProbability(const Eigen::Array3i& index, const float probability) { *mutable_value(index) = mapping::ProbabilityToValue(probability); @@ -515,6 +531,19 @@ class HybridGrid : public HybridGridBase { std::vector update_indices_; }; +inline proto::HybridGrid ToProto(const HybridGrid& grid) { + proto::HybridGrid result; + result.set_resolution(grid.resolution()); + *result.mutable_origin() = transform::ToProto(grid.origin()); + for (const auto it : grid) { + result.add_x_indices(it.first.x()); + result.add_y_indices(it.first.y()); + result.add_z_indices(it.first.z()); + result.add_values(it.second); + } + return result; +} + } // namespace mapping_3d } // namespace cartographer diff --git a/cartographer/mapping_3d/hybrid_grid_test.cc b/cartographer/mapping_3d/hybrid_grid_test.cc index 6b29c34..e3b324f 100644 --- a/cartographer/mapping_3d/hybrid_grid_test.cc +++ b/cartographer/mapping_3d/hybrid_grid_test.cc @@ -16,6 +16,7 @@ #include "cartographer/mapping_3d/hybrid_grid.h" +#include #include #include @@ -125,51 +126,59 @@ TEST(HybridGridTest, GetCenterOfCell) { EXPECT_THAT(hybrid_grid.GetCellIndex(center), AllCwiseEqual(index)); } -TEST(HybridGridTest, TestIteration) { - HybridGrid hybrid_grid(2.f, Eigen::Vector3f(-7.f, -12.f, 0.f)); +class RandomHybridGridTest : public ::testing::Test { + public: + RandomHybridGridTest() + : hybrid_grid_(2.f, Eigen::Vector3f(-7.f, -12.f, 0.f)), values_() { + std::mt19937 rng(1285120005); + std::uniform_real_distribution value_distribution( + mapping::kMinProbability, mapping::kMaxProbability); + std::uniform_int_distribution xyz_distribution(-3000, 2999); + for (int i = 0; i < 10000; ++i) { + const auto x = xyz_distribution(rng); + const auto y = xyz_distribution(rng); + const auto z = xyz_distribution(rng); + values_.emplace(std::make_tuple(x, y, z), value_distribution(rng)); + } - std::map, float> values; - std::mt19937 rng(1285120005); - std::uniform_real_distribution value_distribution( - mapping::kMinProbability, mapping::kMaxProbability); - std::uniform_int_distribution xyz_distribution(-3000, 2999); - for (int i = 0; i < 10000; ++i) { - const auto x = xyz_distribution(rng); - const auto y = xyz_distribution(rng); - const auto z = xyz_distribution(rng); - values.emplace(std::make_tuple(x, y, z), value_distribution(rng)); + for (const auto& pair : values_) { + const Eigen::Array3i cell_index(std::get<0>(pair.first), + std::get<1>(pair.first), + std::get<2>(pair.first)); + hybrid_grid_.SetProbability(cell_index, pair.second); + } } - for (const auto& pair : values) { - const Eigen::Array3i cell_index(std::get<0>(pair.first), - std::get<1>(pair.first), - std::get<2>(pair.first)); - hybrid_grid.SetProbability(cell_index, pair.second); - } + protected: + HybridGrid hybrid_grid_; + using ValueMap = std::map, float>; + ValueMap values_; +}; - for (auto it = HybridGrid::Iterator(hybrid_grid); !it.Done(); it.Next()) { +TEST_F(RandomHybridGridTest, TestIteration) { + for (auto it = HybridGrid::Iterator(hybrid_grid_); !it.Done(); it.Next()) { const Eigen::Array3i cell_index = it.GetCellIndex(); const float iterator_probability = mapping::ValueToProbability(it.GetValue()); - EXPECT_EQ(iterator_probability, hybrid_grid.GetProbability(cell_index)); + EXPECT_EQ(iterator_probability, hybrid_grid_.GetProbability(cell_index)); const std::tuple key = std::make_tuple(cell_index[0], cell_index[1], cell_index[2]); - EXPECT_TRUE(values.count(key)); - EXPECT_NEAR(values[key], iterator_probability, 1e-4); - values.erase(key); + EXPECT_TRUE(values_.count(key)); + EXPECT_NEAR(values_[key], iterator_probability, 1e-4); + values_.erase(key); } // Test that range based loop is equivalent to using the iterator. - auto it = HybridGrid::Iterator(hybrid_grid); - for (const auto& cell : hybrid_grid) { + auto it = HybridGrid::Iterator(hybrid_grid_); + for (const auto& cell : hybrid_grid_) { ASSERT_FALSE(it.Done()); EXPECT_THAT(cell.first, AllCwiseEqual(it.GetCellIndex())); EXPECT_EQ(cell.second, it.GetValue()); it.Next(); } - // Now 'values' must not contain values. - for (const auto& pair : values) { + // Now 'values_' must not contain values. + for (const auto& pair : values_) { const Eigen::Array3i cell_index(std::get<0>(pair.first), std::get<1>(pair.first), std::get<2>(pair.first)); @@ -177,6 +186,56 @@ TEST(HybridGridTest, TestIteration) { } } +TEST_F(RandomHybridGridTest, ToProto) { + const auto proto = ToProto(hybrid_grid_); + EXPECT_EQ(hybrid_grid_.resolution(), proto.resolution()); + EXPECT_EQ(hybrid_grid_.origin().x(), proto.origin().x()); + EXPECT_EQ(hybrid_grid_.origin().y(), proto.origin().y()); + EXPECT_EQ(hybrid_grid_.origin().z(), proto.origin().z()); + + ASSERT_EQ(proto.x_indices_size(), proto.y_indices_size()); + ASSERT_EQ(proto.x_indices_size(), proto.z_indices_size()); + ASSERT_EQ(proto.x_indices_size(), proto.values_size()); + + ValueMap proto_map; + for (int i = 0; i < proto.x_indices_size(); ++i) { + proto_map[std::make_tuple(proto.x_indices(i), proto.y_indices(i), + proto.z_indices(i))] = proto.values(i); + } + + // Get hybrid_grid_ into the same format. + ValueMap hybrid_grid_map; + for (const auto i : hybrid_grid_) { + hybrid_grid_map[std::make_tuple(i.first.x(), i.first.y(), i.first.z())] = + i.second; + } + + EXPECT_EQ(proto_map, hybrid_grid_map); +} + +namespace { + +struct EigenComparator { + bool operator()(const Eigen::Vector3i& lhs, const Eigen::Vector3i& rhs) { + return std::forward_as_tuple(lhs.x(), lhs.y(), lhs.z()) < + std::forward_as_tuple(rhs.x(), rhs.y(), rhs.z()); + } +}; + +} // namespace + +TEST_F(RandomHybridGridTest, FromProto) { + const HybridGrid constructed_grid(ToProto(hybrid_grid_)); + + std::map member_map( + hybrid_grid_.begin(), hybrid_grid_.end()); + + std::map constructed_map( + constructed_grid.begin(), constructed_grid.end()); + + EXPECT_EQ(member_map, constructed_map); +} + } // namespace } // namespace mapping_3d } // namespace cartographer diff --git a/cartographer/mapping_3d/proto/hybrid_grid.proto b/cartographer/mapping_3d/proto/hybrid_grid.proto new file mode 100644 index 0000000..84b523c --- /dev/null +++ b/cartographer/mapping_3d/proto/hybrid_grid.proto @@ -0,0 +1,31 @@ +// Copyright 2016 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +syntax = "proto2"; + +import "cartographer/transform/proto/transform.proto"; + +package cartographer.mapping_3d.proto; + +message HybridGrid { + optional float resolution = 1; + optional transform.proto.Vector3f origin = 2; + // '{x, y, z}_indices[i]' is the index of 'values[i]'. + repeated sint32 x_indices = 3 [packed = true]; + repeated sint32 y_indices = 4 [packed = true]; + repeated sint32 z_indices = 5 [packed = true]; + // The entries in 'values' should be uint16s, not int32s, but protos don't + // have a uint16 type. + repeated int32 values = 6 [packed = true]; +}