diff --git a/cartographer/mapping_2d/map_limits.h b/cartographer/mapping_2d/map_limits.h index 1b9b2ba..c263c8e 100644 --- a/cartographer/mapping_2d/map_limits.h +++ b/cartographer/mapping_2d/map_limits.h @@ -23,8 +23,9 @@ #include "Eigen/Core" #include "Eigen/Geometry" #include "cartographer/common/math.h" -#include "cartographer/mapping/trajectory_node.h" +#include "cartographer/mapping_2d/proto/map_limits.pb.h" #include "cartographer/mapping_2d/xy_index.h" +#include "cartographer/mapping/trajectory_node.h" #include "cartographer/sensor/laser.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/rigid_transform.h" @@ -46,6 +47,11 @@ class MapLimits { CHECK_GT(cell_limits.num_y_cells, 0.); } + explicit MapLimits(const proto::MapLimits& map_limits) + : resolution_(map_limits.resolution()), + max_(transform::ToEigen(map_limits.max())), + cell_limits_(map_limits.cell_limits()) {} + // Returns the cell size in meters. All cells are square and the resolution is // the length of one side. double resolution() const { return resolution_; } @@ -73,9 +79,8 @@ class MapLimits { // Returns true of the ProbabilityGrid contains 'xy_index'. bool Contains(const Eigen::Array2i& xy_index) const { return (Eigen::Array2i(0, 0) <= xy_index).all() && - (xy_index < - Eigen::Array2i(cell_limits_.num_x_cells, cell_limits_.num_y_cells)) - .all(); + (xy_index < Eigen::Array2i(cell_limits_.num_x_cells, + cell_limits_.num_y_cells)).all(); } // Computes MapLimits that contain the origin, and all laser rays (both @@ -129,6 +134,14 @@ class MapLimits { CellLimits cell_limits_; }; +inline proto::MapLimits ToProto(const MapLimits& map_limits) { + proto::MapLimits result; + result.set_resolution(map_limits.resolution()); + *result.mutable_max() = transform::ToProto(map_limits.max()); + *result.mutable_cell_limits() = ToProto(map_limits.cell_limits()); + return result; +} + } // namespace mapping_2d } // namespace cartographer diff --git a/cartographer/mapping_2d/map_limits_test.cc b/cartographer/mapping_2d/map_limits_test.cc index 168c4f0..4f7d2a6 100644 --- a/cartographer/mapping_2d/map_limits_test.cc +++ b/cartographer/mapping_2d/map_limits_test.cc @@ -22,6 +22,32 @@ namespace cartographer { namespace mapping_2d { namespace { +TEST(MapLimitsTest, ToProto) { + const MapLimits limits(42., Eigen::Vector2d(3., 0.), CellLimits(2, 3)); + const auto proto = ToProto(limits); + EXPECT_EQ(limits.resolution(), proto.resolution()); + EXPECT_EQ(limits.max().x(), proto.max().x()); + EXPECT_EQ(limits.max().y(), proto.max().y()); + EXPECT_EQ(ToProto(limits.cell_limits()).DebugString(), + proto.cell_limits().DebugString()); +} + +TEST(MapLimitsTest, ProtoConstructor) { + proto::MapLimits limits; + limits.set_resolution(1.); + limits.mutable_max()->set_x(2.); + limits.mutable_max()->set_y(3.); + limits.mutable_cell_limits()->set_num_x_cells(4); + limits.mutable_cell_limits()->set_num_y_cells(5); + + const MapLimits native(limits); + EXPECT_EQ(limits.resolution(), native.resolution()); + EXPECT_EQ(limits.max().x(), native.max().x()); + EXPECT_EQ(limits.max().y(), native.max().y()); + EXPECT_EQ(limits.cell_limits().DebugString(), + ToProto(native.cell_limits()).DebugString()); +} + TEST(MapLimitsTest, ConstructAndGet) { const MapLimits limits(42., Eigen::Vector2d(3., 0.), CellLimits(2, 3)); diff --git a/cartographer/mapping_2d/probability_grid.h b/cartographer/mapping_2d/probability_grid.h index 7ae4cdd..ffb73e8 100644 --- a/cartographer/mapping_2d/probability_grid.h +++ b/cartographer/mapping_2d/probability_grid.h @@ -27,9 +27,10 @@ #include "cartographer/common/math.h" #include "cartographer/common/port.h" -#include "cartographer/mapping/probability_values.h" #include "cartographer/mapping_2d/map_limits.h" +#include "cartographer/mapping_2d/proto/probability_grid.pb.h" #include "cartographer/mapping_2d/xy_index.h" +#include "cartographer/mapping/probability_values.h" #include "glog/logging.h" namespace cartographer { @@ -48,6 +49,22 @@ class ProbabilityGrid { min_x_(limits_.cell_limits().num_x_cells - 1), min_y_(limits_.cell_limits().num_y_cells - 1) {} + explicit ProbabilityGrid(const proto::ProbabilityGrid& proto) + : limits_(proto.limits()), + cells_(), + update_indices_(proto.update_indices().begin(), + proto.update_indices().end()), + max_x_(proto.max_x()), + max_y_(proto.max_y()), + min_x_(proto.min_x()), + min_y_(proto.min_y()) { + cells_.reserve(proto.cells_size()); + for (const auto cell : proto.cells()) { + CHECK_LE(cell, std::numeric_limits::max()); + cells_.push_back(cell); + } + } + // Returns the limits of this ProbabilityGrid. const MapLimits& limits() const { return limits_; } @@ -106,8 +123,9 @@ class ProbabilityGrid { // Returns true if the probability at the specified index is known. bool IsKnown(const Eigen::Array2i& xy_index) const { - return limits_.Contains(xy_index) && cells_[GetIndexOfCell(xy_index)] != - mapping::kUnknownProbabilityValue; + return limits_.Contains(xy_index) && + cells_[GetIndexOfCell(xy_index)] != + mapping::kUnknownProbabilityValue; } // Fills in 'offset' and 'limits' to define a subregion of that contains all @@ -154,6 +172,24 @@ class ProbabilityGrid { } } + proto::ProbabilityGrid ToProto() { + proto::ProbabilityGrid result; + *result.mutable_limits() = cartographer::mapping_2d::ToProto(limits_); + result.mutable_cells()->Reserve(cells_.size()); + for (const auto cell : cells_) { + result.mutable_cells()->Add(cell); + } + result.mutable_update_indices()->Reserve(update_indices_.size()); + for (const auto update : update_indices_) { + result.mutable_update_indices()->Add(update); + } + result.set_max_x(max_x_); + result.set_max_y(max_y_); + result.set_min_x(min_x_); + result.set_min_y(min_y_); + return result; + } + private: // Returns the index of the cell at 'xy_index'. int GetIndexOfCell(const Eigen::Array2i& xy_index) const { @@ -172,7 +208,7 @@ class ProbabilityGrid { std::vector cells_; // Highest bit is update marker. std::vector update_indices_; - // Minimum and maximum cell coordinates of know cells to efficiently compute + // Minimum and maximum cell coordinates of known cells to efficiently compute // cropping limits. int max_x_; int max_y_; diff --git a/cartographer/mapping_2d/probability_grid_test.cc b/cartographer/mapping_2d/probability_grid_test.cc index cb16685..cc1591c 100644 --- a/cartographer/mapping_2d/probability_grid_test.cc +++ b/cartographer/mapping_2d/probability_grid_test.cc @@ -17,6 +17,7 @@ #include "cartographer/mapping_2d/probability_grid.h" #include +#include #include "gtest/gtest.h" @@ -24,6 +25,40 @@ namespace cartographer { namespace mapping_2d { namespace { +TEST(ProbabilityGridTest, ProtoConstructor) { + proto::ProbabilityGrid proto; + const MapLimits limits(1., {2., 3.}, CellLimits(4., 5.)); + *proto.mutable_limits() = ToProto(limits); + for (int i = 6; i < 12; ++i) { + proto.mutable_cells()->Add(static_cast(i)); + } + for (int i = 13; i < 18; ++i) { + proto.mutable_update_indices()->Add(i); + } + proto.set_max_x(19); + proto.set_max_y(20); + proto.set_min_x(21); + proto.set_min_y(22); + + ProbabilityGrid grid(proto); + EXPECT_EQ(proto.limits().DebugString(), ToProto(grid.limits()).DebugString()); + + // TODO(macmason): Figure out how to test the contents of cells_, + // update_indices_, and {min, max}_{x, y}_ gracefully. +} + +TEST(ProbabilityGridTest, ToProto) { + ProbabilityGrid probability_grid( + MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2))); + + const auto proto = probability_grid.ToProto(); + EXPECT_EQ(ToProto(probability_grid.limits()).DebugString(), + proto.limits().DebugString()); + + // TODO(macmason): Figure out how to test the contents of cells_, + // update_indices_, and {min, max}_{x, y}_ gracefully. +} + TEST(ProbabilityGridTest, ApplyOdds) { ProbabilityGrid probability_grid( MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2))); @@ -111,35 +146,26 @@ TEST(ProbabilityGridTest, GetXYIndexOfCellContainingPoint) { const CellLimits& cell_limits = limits.cell_limits(); ASSERT_EQ(14, cell_limits.num_x_cells); ASSERT_EQ(8, cell_limits.num_y_cells); - EXPECT_TRUE( - (Eigen::Array2i(0, 0) == limits.GetXYIndexOfCellContainingPoint(7, 13)) - .all()); - EXPECT_TRUE( - (Eigen::Array2i(13, 0) == limits.GetXYIndexOfCellContainingPoint(7, -13)) - .all()); - EXPECT_TRUE( - (Eigen::Array2i(0, 7) == limits.GetXYIndexOfCellContainingPoint(-7, 13)) - .all()); - EXPECT_TRUE( - (Eigen::Array2i(13, 7) == limits.GetXYIndexOfCellContainingPoint(-7, -13)) - .all()); + EXPECT_TRUE((Eigen::Array2i(0, 0) == + limits.GetXYIndexOfCellContainingPoint(7, 13)).all()); + EXPECT_TRUE((Eigen::Array2i(13, 0) == + limits.GetXYIndexOfCellContainingPoint(7, -13)).all()); + EXPECT_TRUE((Eigen::Array2i(0, 7) == + limits.GetXYIndexOfCellContainingPoint(-7, 13)).all()); + EXPECT_TRUE((Eigen::Array2i(13, 7) == + limits.GetXYIndexOfCellContainingPoint(-7, -13)).all()); // Check around the origin. - EXPECT_TRUE( - (Eigen::Array2i(6, 3) == limits.GetXYIndexOfCellContainingPoint(0.5, 0.5)) - .all()); - EXPECT_TRUE( - (Eigen::Array2i(6, 3) == limits.GetXYIndexOfCellContainingPoint(1.5, 1.5)) - .all()); + EXPECT_TRUE((Eigen::Array2i(6, 3) == + limits.GetXYIndexOfCellContainingPoint(0.5, 0.5)).all()); + EXPECT_TRUE((Eigen::Array2i(6, 3) == + limits.GetXYIndexOfCellContainingPoint(1.5, 1.5)).all()); EXPECT_TRUE((Eigen::Array2i(7, 3) == - limits.GetXYIndexOfCellContainingPoint(0.5, -0.5)) - .all()); + limits.GetXYIndexOfCellContainingPoint(0.5, -0.5)).all()); EXPECT_TRUE((Eigen::Array2i(6, 4) == - limits.GetXYIndexOfCellContainingPoint(-0.5, 0.5)) - .all()); + limits.GetXYIndexOfCellContainingPoint(-0.5, 0.5)).all()); EXPECT_TRUE((Eigen::Array2i(7, 4) == - limits.GetXYIndexOfCellContainingPoint(-0.5, -0.5)) - .all()); + limits.GetXYIndexOfCellContainingPoint(-0.5, -0.5)).all()); } TEST(ProbabilityGridTest, CorrectCropping) { diff --git a/cartographer/mapping_2d/proto/cell_limits.proto b/cartographer/mapping_2d/proto/cell_limits.proto new file mode 100644 index 0000000..b655d23 --- /dev/null +++ b/cartographer/mapping_2d/proto/cell_limits.proto @@ -0,0 +1,22 @@ +// 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"; + +package cartographer.mapping_2d.proto; + +message CellLimits { + optional int32 num_x_cells = 1; + optional int32 num_y_cells = 2; +} diff --git a/cartographer/mapping_2d/proto/map_limits.proto b/cartographer/mapping_2d/proto/map_limits.proto new file mode 100644 index 0000000..3b868c7 --- /dev/null +++ b/cartographer/mapping_2d/proto/map_limits.proto @@ -0,0 +1,26 @@ +// 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/mapping_2d/proto/cell_limits.proto"; +import "cartographer/transform/proto/transform.proto"; + +package cartographer.mapping_2d.proto; + +message MapLimits { + optional double resolution = 1; + optional cartographer.transform.proto.Vector2d max = 2; + optional CellLimits cell_limits = 3; +} diff --git a/cartographer/mapping_2d/proto/probability_grid.proto b/cartographer/mapping_2d/proto/probability_grid.proto new file mode 100644 index 0000000..7b15a3e --- /dev/null +++ b/cartographer/mapping_2d/proto/probability_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/mapping_2d/proto/map_limits.proto"; + +package cartographer.mapping_2d.proto; + +message ProbabilityGrid { + optional MapLimits limits = 1; + // These values are actually int16s, but protos don't have a native int16 type. + repeated int32 cells = 2; + repeated int32 update_indices = 3; + optional int32 max_x = 4; + optional int32 max_y = 5; + optional int32 min_x = 6; + optional int32 min_y = 7; +} + diff --git a/cartographer/mapping_2d/xy_index.h b/cartographer/mapping_2d/xy_index.h index e190b6e..fae2252 100644 --- a/cartographer/mapping_2d/xy_index.h +++ b/cartographer/mapping_2d/xy_index.h @@ -25,6 +25,7 @@ #include "Eigen/Core" #include "cartographer/common/math.h" #include "cartographer/common/port.h" +#include "cartographer/mapping_2d/proto/cell_limits.pb.h" #include "glog/logging.h" namespace cartographer { @@ -35,10 +36,21 @@ struct CellLimits { CellLimits(int init_num_x_cells, int init_num_y_cells) : num_x_cells(init_num_x_cells), num_y_cells(init_num_y_cells) {} + explicit CellLimits(const proto::CellLimits& cell_limits) + : num_x_cells(cell_limits.num_x_cells()), + num_y_cells(cell_limits.num_y_cells()) {} + int num_x_cells = 0; int num_y_cells = 0; }; +inline proto::CellLimits ToProto(const CellLimits& cell_limits) { + proto::CellLimits result; + result.set_num_x_cells(cell_limits.num_x_cells); + result.set_num_y_cells(cell_limits.num_y_cells); + return result; +} + // Iterates in row-major order through a range of xy-indices. class XYIndexRangeIterator : public std::iterator { diff --git a/cartographer/mapping_2d/xy_index_test.cc b/cartographer/mapping_2d/xy_index_test.cc index 7877ebe..05f9b43 100644 --- a/cartographer/mapping_2d/xy_index_test.cc +++ b/cartographer/mapping_2d/xy_index_test.cc @@ -22,6 +22,23 @@ namespace cartographer { namespace mapping_2d { namespace { +TEST(XYIndexTest, CellLimitsToProto) { + const CellLimits limits(1, 2); + const auto proto = ToProto(limits); + EXPECT_EQ(limits.num_x_cells, proto.num_x_cells()); + EXPECT_EQ(limits.num_y_cells, proto.num_y_cells()); +} + +TEST(XYIndexTest, CellLimitsProtoConstructor) { + proto::CellLimits limits; + limits.set_num_x_cells(1); + limits.set_num_y_cells(2); + + auto native = CellLimits(limits); + EXPECT_EQ(limits.num_x_cells(), native.num_x_cells); + EXPECT_EQ(limits.num_y_cells(), native.num_y_cells); +} + TEST(XYIndexTest, XYIndexRangeIterator) { const Eigen::Array2i min(1, 2); const Eigen::Array2i max(3, 4); diff --git a/cartographer/transform/transform.cc b/cartographer/transform/transform.cc index 641b5a4..7b7f7b7 100644 --- a/cartographer/transform/transform.cc +++ b/cartographer/transform/transform.cc @@ -76,6 +76,13 @@ proto::Rigid3f ToProto(const transform::Rigid3f& rigid) { return proto; } +proto::Vector2d ToProto(const Eigen::Vector2d& vector) { + proto::Vector2d proto; + proto.set_x(vector.x()); + proto.set_y(vector.y()); + return proto; +} + proto::Vector3f ToProto(const Eigen::Vector3f& vector) { proto::Vector3f proto; proto.set_x(vector.x()); diff --git a/cartographer/transform/transform.h b/cartographer/transform/transform.h index cac08bd..5fd3024 100644 --- a/cartographer/transform/transform.h +++ b/cartographer/transform/transform.h @@ -125,6 +125,7 @@ proto::Rigid2f ToProto(const Rigid2f& transform); proto::Rigid3d ToProto(const Rigid3d& rigid); Rigid3d ToRigid3(const proto::Rigid3d& rigid); proto::Rigid3f ToProto(const Rigid3f& rigid); +proto::Vector2d ToProto(const Eigen::Vector2d& vector); proto::Vector3f ToProto(const Eigen::Vector3f& vector); proto::Vector3d ToProto(const Eigen::Vector3d& vector); proto::Quaternionf ToProto(const Eigen::Quaternionf& quaternion);