Adds HybridGrid proto serialization (#220)

master
Mac Mason 2017-03-14 03:44:19 -07:00 committed by Damon Kohler
parent 8bf6f101c5
commit 76868b7edb
3 changed files with 146 additions and 27 deletions

View File

@ -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<uint16> {
HybridGrid(const float resolution, const Eigen::Vector3f& origin)
: HybridGridBase<uint16>(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<uint16> {
std::vector<ValueType*> 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

View File

@ -16,6 +16,7 @@
#include "cartographer/mapping_3d/hybrid_grid.h"
#include <map>
#include <random>
#include <tuple>
@ -125,10 +126,10 @@ 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));
std::map<std::tuple<int, int, int>, float> values;
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<float> value_distribution(
mapping::kMinProbability, mapping::kMaxProbability);
@ -137,39 +138,47 @@ TEST(HybridGridTest, TestIteration) {
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));
values_.emplace(std::make_tuple(x, y, z), value_distribution(rng));
}
for (const auto& pair : 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));
hybrid_grid.SetProbability(cell_index, pair.second);
hybrid_grid_.SetProbability(cell_index, pair.second);
}
}
for (auto it = HybridGrid::Iterator(hybrid_grid); !it.Done(); it.Next()) {
protected:
HybridGrid hybrid_grid_;
using ValueMap = std::map<std::tuple<int, int, int>, float>;
ValueMap values_;
};
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<int, int, int> 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<Eigen::Vector3i, float, EigenComparator> member_map(
hybrid_grid_.begin(), hybrid_grid_.end());
std::map<Eigen::Vector3i, float, EigenComparator> constructed_map(
constructed_grid.begin(), constructed_grid.end());
EXPECT_EQ(member_map, constructed_map);
}
} // namespace
} // namespace mapping_3d
} // namespace cartographer

View File

@ -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];
}