diff --git a/cartographer/mapping/2d/grid_2d.cc b/cartographer/mapping/2d/grid_2d.cc new file mode 100644 index 0000000..1729e5d --- /dev/null +++ b/cartographer/mapping/2d/grid_2d.cc @@ -0,0 +1,147 @@ +/* + * Copyright 2018 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. + */ +#include "cartographer/mapping/2d/grid_2d.h" + +#include "cartographer/mapping/probability_values.h" + +namespace cartographer { +namespace mapping { + +Grid2D::Grid2D(const MapLimits& limits) + : limits_(limits), + correspondence_cost_cells_( + limits_.cell_limits().num_x_cells * limits_.cell_limits().num_y_cells, + kUnknownCorrespondenceValue) {} + +Grid2D::Grid2D(const proto::Grid2D& proto) + : limits_(proto.limits()), correspondence_cost_cells_() { + if (proto.has_known_cells_box()) { + const auto& box = proto.known_cells_box(); + known_cells_box_ = + Eigen::AlignedBox2i(Eigen::Vector2i(box.min_x(), box.min_y()), + Eigen::Vector2i(box.max_x(), box.max_y())); + } + correspondence_cost_cells_.reserve(proto.cells_size()); + for (const auto& cell : proto.cells()) { + CHECK_LE(cell, std::numeric_limits::max()); + correspondence_cost_cells_.push_back(cell); + } +} + +// Finishes the update sequence. +void Grid2D::FinishUpdate() { + while (!update_indices_.empty()) { + DCHECK_GE(correspondence_cost_cells_[update_indices_.back()], + kUpdateMarker); + correspondence_cost_cells_[update_indices_.back()] -= kUpdateMarker; + update_indices_.pop_back(); + } +} + +// Sets the probability of the cell at 'cell_index' to the given +// 'probability'. Only allowed if the cell was unknown before. +void Grid2D::SetCorrespondenceCost(const Eigen::Array2i& cell_index, + const float correspondence_cost) { + LOG(FATAL) << "Not implemented"; +} + +// Returns the probability of the cell with 'cell_index'. +float Grid2D::GetCorrespondenceCost(const Eigen::Array2i& cell_index) const { + LOG(FATAL) << "Not implemented"; + return kUnknownCorrespondenceValue; +} + +// Returns true if the probability at the specified index is known. +bool Grid2D::IsKnown(const Eigen::Array2i& cell_index) const { + return limits_.Contains(cell_index) && + correspondence_cost_cells_[ToFlatIndex(cell_index)] != + kUnknownCorrespondenceValue; +} + +// Fills in 'offset' and 'limits' to define a subregion of that contains all +// known cells. +void Grid2D::ComputeCroppedLimits(Eigen::Array2i* const offset, + CellLimits* const limits) const { + if (known_cells_box_.isEmpty()) { + *offset = Eigen::Array2i::Zero(); + *limits = CellLimits(1, 1); + return; + } + *offset = known_cells_box_.min().array(); + *limits = CellLimits(known_cells_box_.sizes().x() + 1, + known_cells_box_.sizes().y() + 1); +} + +// Grows the map as necessary to include 'point'. This changes the meaning of +// these coordinates going forward. This method must be called immediately +// after 'FinishUpdate', before any calls to 'ApplyLookupTable'. +void Grid2D::GrowLimits(const Eigen::Vector2f& point) { + CHECK(update_indices_.empty()); + while (!limits_.Contains(limits_.GetCellIndex(point))) { + const int x_offset = limits_.cell_limits().num_x_cells / 2; + const int y_offset = limits_.cell_limits().num_y_cells / 2; + const MapLimits new_limits( + limits_.resolution(), + limits_.max() + + limits_.resolution() * Eigen::Vector2d(y_offset, x_offset), + CellLimits(2 * limits_.cell_limits().num_x_cells, + 2 * limits_.cell_limits().num_y_cells)); + const int stride = new_limits.cell_limits().num_x_cells; + const int offset = x_offset + stride * y_offset; + const int new_size = new_limits.cell_limits().num_x_cells * + new_limits.cell_limits().num_y_cells; + std::vector new_cells(new_size, kUnknownCorrespondenceValue); + for (int i = 0; i < limits_.cell_limits().num_y_cells; ++i) { + for (int j = 0; j < limits_.cell_limits().num_x_cells; ++j) { + new_cells[offset + j + i * stride] = + correspondence_cost_cells_[j + + i * limits_.cell_limits().num_x_cells]; + } + } + correspondence_cost_cells_ = new_cells; + limits_ = new_limits; + if (!known_cells_box_.isEmpty()) { + known_cells_box_.translate(Eigen::Vector2i(x_offset, y_offset)); + } + } +} + +proto::Grid2D Grid2D::ToProto() const { + proto::Grid2D result; + *result.mutable_limits() = mapping::ToProto(limits_); + result.mutable_cells()->Reserve(correspondence_cost_cells_.size()); + for (const auto& cell : correspondence_cost_cells_) { + result.mutable_cells()->Add(cell); + } + CHECK(update_indices().empty()) << "Serializing a grid during an update is " + "not supported. Finish the update first."; + if (!known_cells_box().isEmpty()) { + auto* const box = result.mutable_known_cells_box(); + box->set_max_x(known_cells_box().max().x()); + box->set_max_y(known_cells_box().max().y()); + box->set_min_x(known_cells_box().min().x()); + box->set_min_y(known_cells_box().min().y()); + } + return result; +} + +int Grid2D::ToFlatIndex(const Eigen::Array2i& cell_index) const { + CHECK(limits_.Contains(cell_index)) << cell_index; + return limits_.cell_limits().num_x_cells * cell_index.y() + cell_index.x(); +} + +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/mapping/2d/grid_2d.h b/cartographer/mapping/2d/grid_2d.h new file mode 100644 index 0000000..45393c0 --- /dev/null +++ b/cartographer/mapping/2d/grid_2d.h @@ -0,0 +1,92 @@ +/* + * Copyright 2018 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. + */ + +#ifndef CARTOGRAPHER_MAPPING_2D_GRID_2D_H_ +#define CARTOGRAPHER_MAPPING_2D_GRID_2D_H_ + +#include + +#include "cartographer/mapping/2d/map_limits.h" +#include "cartographer/mapping/2d/proto/grid_2d.pb.h" + +namespace cartographer { +namespace mapping { + +class Grid2D { + public: + explicit Grid2D(const MapLimits& limits); + explicit Grid2D(const proto::Grid2D& proto); + + // Returns the limits of this ProbabilityGrid. + const MapLimits& limits() const { return limits_; } + + // Finishes the update sequence. + void FinishUpdate(); + + // Sets the correspondence cost of the cell at 'cell_index' to the given + // 'correspondence cost'. Only allowed if the cell was unknown before. + void SetCorrespondenceCost(const Eigen::Array2i& cell_index, + const float correspondence_cost); + + // Returns the probability of the cell with 'cell_index'. + float GetCorrespondenceCost(const Eigen::Array2i& cell_index) const; + + // Returns true if the probability at the specified index is known. + bool IsKnown(const Eigen::Array2i& cell_index) const; + + // Fills in 'offset' and 'limits' to define a subregion of that contains all + // known cells. + void ComputeCroppedLimits(Eigen::Array2i* const offset, + CellLimits* const limits) const; + + // Grows the map as necessary to include 'point'. This changes the meaning of + // these coordinates going forward. This method must be called immediately + // after 'FinishUpdate', before any calls to 'ApplyLookupTable'. + void GrowLimits(const Eigen::Vector2f& point); + + virtual proto::Grid2D ToProto() const; + + protected: + const std::vector& correspondence_cost_cells() const { + return correspondence_cost_cells_; + } + const std::vector& update_indices() const { return update_indices_; } + const Eigen::AlignedBox2i& known_cells_box() const { + return known_cells_box_; + } + + std::vector* mutable_correspondence_cost_cells() { + return &correspondence_cost_cells_; + } + std::vector* mutable_update_indices() { return &update_indices_; } + Eigen::AlignedBox2i* mutable_known_cells_box() { return &known_cells_box_; } + + // Converts a 'cell_index' into an index into 'cells_'. + int ToFlatIndex(const Eigen::Array2i& cell_index) const; + + private: + MapLimits limits_; + std::vector correspondence_cost_cells_; + std::vector update_indices_; + + // Bounding box of known cells to efficiently compute cropping limits. + Eigen::AlignedBox2i known_cells_box_; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_2D_GRID_2D_H_ diff --git a/cartographer/mapping/2d/probability_grid.cc b/cartographer/mapping/2d/probability_grid.cc index dec5a89..ecd8404 100644 --- a/cartographer/mapping/2d/probability_grid.cc +++ b/cartographer/mapping/2d/probability_grid.cc @@ -1,3 +1,18 @@ +/* + * 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. + */ #include "cartographer/mapping/2d/probability_grid.h" #include @@ -6,54 +21,22 @@ namespace cartographer { namespace mapping { -namespace { -// Converts a 'cell_index' into an index into 'cells_'. -int ToFlatIndex(const Eigen::Array2i& cell_index, const MapLimits& limits) { - CHECK(limits.Contains(cell_index)) << cell_index; - return limits.cell_limits().num_x_cells * cell_index.y() + cell_index.x(); -} +ProbabilityGrid::ProbabilityGrid(const MapLimits& limits) : Grid2D(limits) {} -} // namespace - -ProbabilityGrid::ProbabilityGrid(const MapLimits& limits) - : limits_(limits), - cells_( - limits_.cell_limits().num_x_cells * limits_.cell_limits().num_y_cells, - kUnknownProbabilityValue) {} - -ProbabilityGrid::ProbabilityGrid(const proto::ProbabilityGrid& proto) - : limits_(proto.limits()), cells_() { - if (proto.has_known_cells_box()) { - const auto& box = proto.known_cells_box(); - known_cells_box_ = - Eigen::AlignedBox2i(Eigen::Vector2i(box.min_x(), box.min_y()), - Eigen::Vector2i(box.max_x(), box.max_y())); - } - cells_.reserve(proto.cells_size()); - for (const auto& cell : proto.cells()) { - CHECK_LE(cell, std::numeric_limits::max()); - cells_.push_back(cell); - } -} - -// Finishes the update sequence. -void ProbabilityGrid::FinishUpdate() { - while (!update_indices_.empty()) { - DCHECK_GE(cells_[update_indices_.back()], kUpdateMarker); - cells_[update_indices_.back()] -= kUpdateMarker; - update_indices_.pop_back(); - } +ProbabilityGrid::ProbabilityGrid(const proto::Grid2D& proto) : Grid2D(proto) { + CHECK(proto.has_probability_grid_2d()); } // Sets the probability of the cell at 'cell_index' to the given // 'probability'. Only allowed if the cell was unknown before. void ProbabilityGrid::SetProbability(const Eigen::Array2i& cell_index, const float probability) { - uint16& cell = cells_[ToFlatIndex(cell_index, limits_)]; + uint16& cell = + (*mutable_correspondence_cost_cells())[ToFlatIndex(cell_index)]; CHECK_EQ(cell, kUnknownProbabilityValue); cell = ProbabilityToValue(probability); - known_cells_box_.extend(cell_index.matrix()); + mutable_known_cells_box()->extend(cell_index.matrix()); } // Applies the 'odds' specified when calling ComputeLookupTableToApplyOdds() @@ -66,95 +49,28 @@ void ProbabilityGrid::SetProbability(const Eigen::Array2i& cell_index, bool ProbabilityGrid::ApplyLookupTable(const Eigen::Array2i& cell_index, const std::vector& table) { DCHECK_EQ(table.size(), kUpdateMarker); - const int flat_index = ToFlatIndex(cell_index, limits_); - uint16* cell = &cells_[flat_index]; + const int flat_index = ToFlatIndex(cell_index); + uint16* cell = &(*mutable_correspondence_cost_cells())[flat_index]; if (*cell >= kUpdateMarker) { return false; } - update_indices_.push_back(flat_index); + mutable_update_indices()->push_back(flat_index); *cell = table[*cell]; DCHECK_GE(*cell, kUpdateMarker); - known_cells_box_.extend(cell_index.matrix()); + mutable_known_cells_box()->extend(cell_index.matrix()); return true; } // Returns the probability of the cell with 'cell_index'. float ProbabilityGrid::GetProbability(const Eigen::Array2i& cell_index) const { - if (limits_.Contains(cell_index)) { - return ValueToProbability(cells_[ToFlatIndex(cell_index, limits_)]); - } - return kMinProbability; + if (!limits().Contains(cell_index)) return kMinProbability; + return ValueToProbability( + correspondence_cost_cells()[ToFlatIndex(cell_index)]); } - -// Returns true if the probability at the specified index is known. -bool ProbabilityGrid::IsKnown(const Eigen::Array2i& cell_index) const { - return limits_.Contains(cell_index) && - cells_[ToFlatIndex(cell_index, limits_)] != kUnknownProbabilityValue; -} - -// Fills in 'offset' and 'limits' to define a subregion of that contains all -// known cells. -void ProbabilityGrid::ComputeCroppedLimits(Eigen::Array2i* const offset, - CellLimits* const limits) const { - if (known_cells_box_.isEmpty()) { - *offset = Eigen::Array2i::Zero(); - *limits = CellLimits(1, 1); - return; - } - *offset = known_cells_box_.min().array(); - *limits = CellLimits(known_cells_box_.sizes().x() + 1, - known_cells_box_.sizes().y() + 1); -} - -// Grows the map as necessary to include 'point'. This changes the meaning of -// these coordinates going forward. This method must be called immediately -// after 'FinishUpdate', before any calls to 'ApplyLookupTable'. -void ProbabilityGrid::GrowLimits(const Eigen::Vector2f& point) { - CHECK(update_indices_.empty()); - while (!limits_.Contains(limits_.GetCellIndex(point))) { - const int x_offset = limits_.cell_limits().num_x_cells / 2; - const int y_offset = limits_.cell_limits().num_y_cells / 2; - const MapLimits new_limits( - limits_.resolution(), - limits_.max() + - limits_.resolution() * Eigen::Vector2d(y_offset, x_offset), - CellLimits(2 * limits_.cell_limits().num_x_cells, - 2 * limits_.cell_limits().num_y_cells)); - const int stride = new_limits.cell_limits().num_x_cells; - const int offset = x_offset + stride * y_offset; - const int new_size = new_limits.cell_limits().num_x_cells * - new_limits.cell_limits().num_y_cells; - std::vector new_cells(new_size, kUnknownProbabilityValue); - for (int i = 0; i < limits_.cell_limits().num_y_cells; ++i) { - for (int j = 0; j < limits_.cell_limits().num_x_cells; ++j) { - new_cells[offset + j + i * stride] = - cells_[j + i * limits_.cell_limits().num_x_cells]; - } - } - cells_ = new_cells; - limits_ = new_limits; - if (!known_cells_box_.isEmpty()) { - known_cells_box_.translate(Eigen::Vector2i(x_offset, y_offset)); - } - } -} - -proto::ProbabilityGrid ProbabilityGrid::ToProto() const { - proto::ProbabilityGrid result; - *result.mutable_limits() = mapping::ToProto(limits_); - result.mutable_cells()->Reserve(cells_.size()); - for (const auto& cell : cells_) { - result.mutable_cells()->Add(cell); - } - CHECK(update_indices_.empty()) << "Serializing a grid during an update is " - "not supported. Finish the update first."; - if (!known_cells_box_.isEmpty()) { - auto* const box = result.mutable_known_cells_box(); - box->set_max_x(known_cells_box_.max().x()); - box->set_max_y(known_cells_box_.max().y()); - box->set_min_x(known_cells_box_.min().x()); - box->set_min_y(known_cells_box_.min().y()); - } +proto::Grid2D ProbabilityGrid::ToProto() const { + proto::Grid2D result; + result = Grid2D::ToProto(); + result.mutable_probability_grid_2d(); return result; } diff --git a/cartographer/mapping/2d/probability_grid.h b/cartographer/mapping/2d/probability_grid.h index 7723f2e..1764e57 100644 --- a/cartographer/mapping/2d/probability_grid.h +++ b/cartographer/mapping/2d/probability_grid.h @@ -20,24 +20,18 @@ #include #include "cartographer/common/port.h" +#include "cartographer/mapping/2d/grid_2d.h" #include "cartographer/mapping/2d/map_limits.h" -#include "cartographer/mapping/2d/proto/probability_grid.pb.h" #include "cartographer/mapping/2d/xy_index.h" namespace cartographer { namespace mapping { // Represents a 2D grid of probabilities. -class ProbabilityGrid { +class ProbabilityGrid : public Grid2D { public: explicit ProbabilityGrid(const MapLimits& limits); - explicit ProbabilityGrid(const proto::ProbabilityGrid& proto); - - // Returns the limits of this ProbabilityGrid. - const MapLimits& limits() const { return limits_; } - - // Finishes the update sequence. - void FinishUpdate(); + explicit ProbabilityGrid(const proto::Grid2D& proto); // Sets the probability of the cell at 'cell_index' to the given // 'probability'. Only allowed if the cell was unknown before. @@ -57,28 +51,7 @@ class ProbabilityGrid { // Returns the probability of the cell with 'cell_index'. float GetProbability(const Eigen::Array2i& cell_index) const; - // Returns true if the probability at the specified index is known. - bool IsKnown(const Eigen::Array2i& cell_index) const; - - // Fills in 'offset' and 'limits' to define a subregion of that contains all - // known cells. - void ComputeCroppedLimits(Eigen::Array2i* const offset, - CellLimits* const limits) const; - - // Grows the map as necessary to include 'point'. This changes the meaning of - // these coordinates going forward. This method must be called immediately - // after 'FinishUpdate', before any calls to 'ApplyLookupTable'. - void GrowLimits(const Eigen::Vector2f& point); - - proto::ProbabilityGrid ToProto() const; - - private: - MapLimits limits_; - std::vector cells_; // Highest bit is update marker. - std::vector update_indices_; - - // Bounding box of known cells to efficiently compute cropping limits. - Eigen::AlignedBox2i known_cells_box_; + virtual proto::Grid2D ToProto() const override; }; } // namespace mapping diff --git a/cartographer/mapping/2d/probability_grid_test.cc b/cartographer/mapping/2d/probability_grid_test.cc index 1cedc46..b93b7dc 100644 --- a/cartographer/mapping/2d/probability_grid_test.cc +++ b/cartographer/mapping/2d/probability_grid_test.cc @@ -29,7 +29,7 @@ using Eigen::Array2i; using Eigen::Vector2f; TEST(ProbabilityGridTest, ProtoConstructor) { - proto::ProbabilityGrid proto; + proto::Grid2D proto; const MapLimits limits(1., {2., 3.}, CellLimits(4., 5.)); *proto.mutable_limits() = ToProto(limits); for (int i = 6; i < 12; ++i) { @@ -39,6 +39,7 @@ TEST(ProbabilityGridTest, ProtoConstructor) { proto.mutable_known_cells_box()->set_max_y(20); proto.mutable_known_cells_box()->set_min_x(21); proto.mutable_known_cells_box()->set_min_y(22); + proto.mutable_probability_grid_2d(); ProbabilityGrid grid(proto); EXPECT_EQ(proto.limits().DebugString(), ToProto(grid.limits()).DebugString()); diff --git a/cartographer/mapping/2d/proto/grid_2d.proto b/cartographer/mapping/2d/proto/grid_2d.proto new file mode 100644 index 0000000..62de74f --- /dev/null +++ b/cartographer/mapping/2d/proto/grid_2d.proto @@ -0,0 +1,38 @@ +// Copyright 2018 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 = "proto3"; + +package cartographer.mapping.proto; + +import "cartographer/mapping/2d/proto/probability_grid.proto"; +import "cartographer/mapping/2d/proto/map_limits.proto"; + +message Grid2D { + message CellBox { + int32 max_x = 1; + int32 max_y = 2; + int32 min_x = 3; + int32 min_y = 4; + } + + MapLimits limits = 1; + // These values are actually int16s, but protos don't have a native int16 + // type. + repeated int32 cells = 2; + CellBox known_cells_box = 3; + oneof grid { + ProbabilityGrid probability_grid_2d = 4; + } +} \ No newline at end of file diff --git a/cartographer/mapping/2d/proto/probability_grid.proto b/cartographer/mapping/2d/proto/probability_grid.proto index 7620249..2a2f1b6 100644 --- a/cartographer/mapping/2d/proto/probability_grid.proto +++ b/cartographer/mapping/2d/proto/probability_grid.proto @@ -14,21 +14,7 @@ syntax = "proto3"; -import "cartographer/mapping/2d/proto/map_limits.proto"; - package cartographer.mapping.proto; message ProbabilityGrid { - message CellBox { - int32 max_x = 1; - int32 max_y = 2; - int32 min_x = 3; - int32 min_y = 4; - } - - MapLimits limits = 1; - // These values are actually int16s, but protos don't have a native int16 - // type. - repeated int32 cells = 2; - CellBox known_cells_box = 8; } diff --git a/cartographer/mapping/2d/submap_2d.cc b/cartographer/mapping/2d/submap_2d.cc index b769c9f..a6d9723 100644 --- a/cartographer/mapping/2d/submap_2d.cc +++ b/cartographer/mapping/2d/submap_2d.cc @@ -69,7 +69,7 @@ Submap2D::Submap2D(const MapLimits& limits, const Eigen::Vector2f& origin) Submap2D::Submap2D(const proto::Submap2D& proto) : Submap(transform::ToRigid3(proto.local_pose())), - probability_grid_(ProbabilityGrid(proto.probability_grid())) { + probability_grid_(ProbabilityGrid(proto.grid())) { set_num_range_data(proto.num_range_data()); set_finished(proto.finished()); } @@ -81,7 +81,7 @@ void Submap2D::ToProto(proto::Submap* const proto, submap_2d->set_num_range_data(num_range_data()); submap_2d->set_finished(finished()); if (include_probability_grid_data) { - *submap_2d->mutable_probability_grid() = probability_grid_.ToProto(); + *submap_2d->mutable_grid() = probability_grid_.ToProto(); } } @@ -90,8 +90,8 @@ void Submap2D::UpdateFromProto(const proto::Submap& proto) { const auto& submap_2d = proto.submap_2d(); set_num_range_data(submap_2d.num_range_data()); set_finished(submap_2d.finished()); - if (submap_2d.has_probability_grid()) { - probability_grid_ = ProbabilityGrid(submap_2d.probability_grid()); + if (submap_2d.has_grid()) { + probability_grid_ = ProbabilityGrid(submap_2d.grid()); } } diff --git a/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d_test.cc b/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d_test.cc index 39cc6d6..b7c7f41 100644 --- a/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d_test.cc +++ b/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d_test.cc @@ -47,24 +47,26 @@ class OverlappingSubmapsTrimmer2DTest : public ::testing::Test { submap_2d.set_finished(is_finished); *submap_2d.mutable_local_pose() = transform::ToProto(pose_3d); - auto* probability_grid = submap_2d.mutable_probability_grid(); + auto* grid = submap_2d.mutable_grid(); for (int i = 0; i < num_cells * num_cells; ++i) { - probability_grid->add_cells(1); + grid->add_cells(1); } - auto* map_limits = probability_grid->mutable_limits(); + auto* map_limits = grid->mutable_limits(); map_limits->set_resolution(1.0); *map_limits->mutable_max() = transform::ToProto(Eigen::Vector2d(num_cells, num_cells)); map_limits->mutable_cell_limits()->set_num_x_cells(num_cells); map_limits->mutable_cell_limits()->set_num_y_cells(num_cells); - auto* know_cells_box = probability_grid->mutable_known_cells_box(); + auto* know_cells_box = grid->mutable_known_cells_box(); know_cells_box->set_min_x(0); know_cells_box->set_min_y(0); know_cells_box->set_max_x(num_cells - 1); know_cells_box->set_max_y(num_cells - 1); + grid->mutable_probability_grid_2d(); + LOG(INFO) << submap_2d.DebugString(); fake_pose_graph_.mutable_submap_data()->Insert( {0 /* trajectory_id */, submap_index}, {std::make_shared(submap_2d), pose_3d}); diff --git a/cartographer/mapping/probability_values.h b/cartographer/mapping/probability_values.h index c1993ba..d9a6e53 100644 --- a/cartographer/mapping/probability_values.h +++ b/cartographer/mapping/probability_values.h @@ -44,6 +44,7 @@ inline float ClampProbability(const float probability) { } constexpr uint16 kUnknownProbabilityValue = 0; +constexpr uint16 kUnknownCorrespondenceValue = 0; constexpr uint16 kUpdateMarker = 1u << 15; // Converts a probability to a uint16 in the [1, 32767] range. diff --git a/cartographer/mapping/proto/submap.proto b/cartographer/mapping/proto/submap.proto index 48db24d..75e1a12 100644 --- a/cartographer/mapping/proto/submap.proto +++ b/cartographer/mapping/proto/submap.proto @@ -16,7 +16,7 @@ syntax = "proto3"; package cartographer.mapping.proto; -import "cartographer/mapping/2d/proto/probability_grid.proto"; +import "cartographer/mapping/2d/proto/grid_2d.proto"; import "cartographer/mapping/3d/proto/hybrid_grid.proto"; import "cartographer/transform/proto/transform.proto"; @@ -25,7 +25,7 @@ message Submap2D { transform.proto.Rigid3d local_pose = 1; int32 num_range_data = 2; bool finished = 3; - ProbabilityGrid probability_grid = 4; + Grid2D grid = 4; } // Serialized state of a Submap3D.