Adds HybridGrid proto serialization (#220)
parent
8bf6f101c5
commit
76868b7edb
|
@ -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
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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];
|
||||
}
|
Loading…
Reference in New Issue