$ rostopic type /submap_list | rosmsg show
std_msgs/Header header
uint32 seq
time stamp
string frame_id
cartographer_ros_msgs/TrajectorySubmapList[] trajectory
cartographer_ros_msgs/SubmapEntry[] submap
int32 submap_version
geometry_msgs/Pose pose
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
2. SubmapQuery
传输可显示的地图,地图数据cells通过gzipped压缩
通过查询方式实现
定义
message SubmapQuery {
message Request {
// Index into 'SubmapList.trajectory(trajectory_id).submap'.
optional int32 submap_index = 1;
// Index into 'TrajectoryList.trajectory'.
optional int32 trajectory_id = 2;
}
message Response {
// Version of the given submap, higher means newer.
// 插入到此Submap中的RangeData个数
optional int32 submap_version = 2;
// GZipped map data, in row-major order, starting with (0,0). Each cell
// consists of two bytes: value (premultiplied by alpha) and alpha.
optional bytes cells = 3;
// Dimensions of the grid in cells.
// 概率值大于0.501的所有Cells围成的最大box
optional int32 width = 4;
optional int32 height = 5;
// Size of one cell in meters(高分辨率3D网格的分辨率).
optional double resolution = 6;
// Pose of the resolution*width x resolution*height rectangle in the submap
// frame.
optional transform.proto.Rigid3d slice_pose = 9;
// Error message in response to malformed requests.
optional string error_message = 8;
}
optional Request request = 1;
optional Response response = 2;
}
class Submaps : public mapping::Submaps {
public:
explicit Submaps(const proto::SubmapsOptions& options);
Submaps(const Submaps&) = delete;
Submaps& operator=(const Submaps&) = delete;
const Submap* Get(int index) const override;
int size() const override;
// Inserts 'range_data' into the Submap collection. 'gravity_alignment' is
// used for the orientation of new submaps so that the z axis approximately
// aligns with gravity.
void InsertRangeData(const sensor::RangeData& range_data,
const Eigen::Quaterniond& gravity_alignment);
private:
void AddSubmap(const transform::Rigid3d& local_pose);
const proto::SubmapsOptions options_;
// 'submaps_' contains pointers, so that resizing the vector does not
// invalidate handed out Submap* pointers.
std::vector<std::unique_ptr<Submap>> submaps_;
RangeDataInserter range_data_inserter_;
};
4.4 HybridGrid
包含使用15位存储概率值的网格,还有1位用于存储对应体素的更新标记
// A grid containing probability values stored using 15 bits, and an update
// marker per voxel.
class HybridGrid : public HybridGridBase<uint16> {
public:
explicit HybridGrid(const float resolution)
: HybridGridBase<uint16>(resolution) {}
explicit HybridGrid(const proto::HybridGrid& proto)
: HybridGrid(proto.resolution()) {
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);
}
// Starts the next update sequence.
void StartUpdate() {
while (!update_indices_.empty()) {
DCHECK_GE(*update_indices_.back(), mapping::kUpdateMarker);
*update_indices_.back() -= mapping::kUpdateMarker;
update_indices_.pop_back();
}
}
// Applies the 'odds' specified when calling ComputeLookupTableToApplyOdds()
// to the probability of the cell at 'index' if the cell has not already been
// updated. Multiple updates of the same cell will be ignored until
// StartUpdate() is called. Returns true if the cell was updated.
//
// If this is the first call to ApplyOdds() for the specified cell, its value
// will be set to probability corresponding to 'odds'.
bool ApplyLookupTable(const Eigen::Array3i& index,
const std::vector<uint16>& table) {
DCHECK_EQ(table.size(), mapping::kUpdateMarker);
uint16* const cell = mutable_value(index);
if (*cell >= mapping::kUpdateMarker) {
return false;
}
update_indices_.push_back(cell);
*cell = table[*cell];
DCHECK_GE(*cell, mapping::kUpdateMarker);
return true;
}
// Returns the probability of the cell with 'index'.
float GetProbability(const Eigen::Array3i& index) const {
return mapping::ValueToProbability(value(index));
}
// Returns true if the probability at the specified 'index' is known.
bool IsKnown(const Eigen::Array3i& index) const { return value(index) != 0; }
private:
// Markers at changed cells.
std::vector<ValueType*> update_indices_;
};
4.5 HybridGridBase
将3D网格表示为宽而浅的树
// Represents a 3D grid as a wide, shallow tree.
template <typename ValueType>
class HybridGridBase : public Grid<ValueType> {
public:
using Iterator = typename Grid<ValueType>::Iterator;
// Creates a new tree-based probability grid with voxels having edge length
// 'resolution' around the origin which becomes the center of the cell at
// index (0, 0, 0).
explicit HybridGridBase(const float resolution) : resolution_(resolution) {}
float resolution() const { return resolution_; }
// Returns the index of the cell containing the 'point'. Indices are integer
// vectors identifying cells, for this the coordinates are rounded to the next
// multiple of the resolution.
Eigen::Array3i GetCellIndex(const Eigen::Vector3f& point) const {
Eigen::Array3f index = point.array() / resolution_;
return Eigen::Array3i(common::RoundToInt(index.x()),
common::RoundToInt(index.y()),
common::RoundToInt(index.z()));
}
// Returns one of the octants, (0, 0, 0), (1, 0, 0), ..., (1, 1, 1).
static Eigen::Array3i GetOctant(const int i) {
DCHECK_GE(i, 0);
DCHECK_LT(i, 8);
return Eigen::Array3i(static_cast<bool>(i & 1), static_cast<bool>(i & 2),
static_cast<bool>(i & 4));
}
// Returns the center of the cell at 'index'.
Eigen::Vector3f GetCenterOfCell(const Eigen::Array3i& index) const {
return index.matrix().cast<float>() * resolution_;
}
// Iterator functions for range-based for loops.
Iterator begin() const { return Iterator(*this); }
Iterator end() const {
Iterator it(*this);
it.AdvanceToEnd();
return it;
}
private:
// Edge length of each voxel. (每个体素的边长)
const float resolution_;
};
4.6 Grid
在3D Submap中,ValueType为uint16
template <typename ValueType>
using Grid = DynamicGrid<NestedGrid<FlatGrid<ValueType, 3>, 3>>;
4.7 DynamicGrid
此动态网格最初由“WrappedGrid”类型的2x2x2格子组成
通过’mutable_value()'在第一次访问时构造Wrapped grids
如有必要,网格增长到每个维度的两倍大小
索引值范围(几乎)围绕原点对称,即允许负索引。
// A grid consisting of 2x2x2 grids of type 'WrappedGrid' initially. Wrapped
// grids are constructed on first access via 'mutable_value()'. If necessary,
// the grid grows to twice the size in each dimension. The range of indices is
// (almost) symmetric around the origin, i.e. negative indices are allowed.
template <typename WrappedGrid>
class DynamicGrid {
public:
using ValueType = typename WrappedGrid::ValueType;
DynamicGrid() : bits_(1), meta_cells_(8) {}
DynamicGrid(DynamicGrid&&) = default;
DynamicGrid& operator=(DynamicGrid&&) = default;
// Returns the current number of voxels per dimension.
// 返回每个维度的当前体素数
int grid_size() const { return WrappedGrid::grid_size() << bits_; }
// Returns the value stored at 'index'.
ValueType value(const Eigen::Array3i& index) const {
const Eigen::Array3i shifted_index = index + (grid_size() >> 1);
// The cast to unsigned is for performance to check with 3 comparisons
// shifted_index.[xyz] >= 0 and shifted_index.[xyz] < grid_size.
if ((shifted_index.cast<unsigned int>() >= grid_size()).any()) {
return ValueType();
}
const Eigen::Array3i meta_index = GetMetaIndex(shifted_index);
const WrappedGrid* const meta_cell =
meta_cells_[ToFlatIndex(meta_index, bits_)].get();
if (meta_cell == nullptr) {
return ValueType();
}
const Eigen::Array3i inner_index =
shifted_index - meta_index * WrappedGrid::grid_size();
return meta_cell->value(inner_index);
}
// Returns a pointer to the value at 'index' to allow changing it, dynamically
// growing the DynamicGrid and constructing new WrappedGrids as needed.
ValueType* mutable_value(const Eigen::Array3i& index) {
const Eigen::Array3i shifted_index = index + (grid_size() >> 1);
// The cast to unsigned is for performance to check with 3 comparisons
// shifted_index.[xyz] >= 0 and shifted_index.[xyz] < grid_size.
if ((shifted_index.cast<unsigned int>() >= grid_size()).any()) {
Grow();
return mutable_value(index);
}
const Eigen::Array3i meta_index = GetMetaIndex(shifted_index);
std::unique_ptr<WrappedGrid>& meta_cell =
meta_cells_[ToFlatIndex(meta_index, bits_)];
if (meta_cell == nullptr) {
meta_cell = common::make_unique<WrappedGrid>();
}
const Eigen::Array3i inner_index =
shifted_index - meta_index * WrappedGrid::grid_size();
return meta_cell->mutable_value(inner_index);
}
// An iterator for iterating over all values not comparing equal to the
// default constructed value.
class Iterator {
public:
explicit Iterator(const DynamicGrid& dynamic_grid)
: bits_(dynamic_grid.bits_),
current_(dynamic_grid.meta_cells_.data()),
end_(dynamic_grid.meta_cells_.data() +
dynamic_grid.meta_cells_.size()),
nested_iterator_() {
AdvanceToValidNestedIterator();
}
void Next() {
DCHECK(!Done());
nested_iterator_.Next();
if (!nested_iterator_.Done()) {
return;
}
++current_;
AdvanceToValidNestedIterator();
}
bool Done() const { return current_ == end_; }
Eigen::Array3i GetCellIndex() const {
DCHECK(!Done());
const int outer_index = (1 << (3 * bits_)) - (end_ - current_);
const Eigen::Array3i shifted_index =
To3DIndex(outer_index, bits_) * WrappedGrid::grid_size() +
nested_iterator_.GetCellIndex();
return shifted_index - ((1 << (bits_ - 1)) * WrappedGrid::grid_size());
}
const ValueType& GetValue() const {
DCHECK(!Done());
return nested_iterator_.GetValue();
}
void AdvanceToEnd() { current_ = end_; }
const std::pair<Eigen::Array3i, ValueType> operator*() const {
return std::pair<Eigen::Array3i, ValueType>(GetCellIndex(), GetValue());
}
Iterator& operator++() {
Next();
return *this;
}
bool operator!=(const Iterator& it) const {
return it.current_ != current_;
}
private:
void AdvanceToValidNestedIterator() {
for (; !Done(); ++current_) {
if (*current_ != nullptr) {
nested_iterator_ = typename WrappedGrid::Iterator(**current_);
if (!nested_iterator_.Done()) {
break;
}
}
}
}
int bits_;
const std::unique_ptr<WrappedGrid>* current_;
const std::unique_ptr<WrappedGrid>* const end_;
typename WrappedGrid::Iterator nested_iterator_;
};
private:
// Returns the Eigen::Array3i (meta) index of the meta cell containing
// 'index'. (返回包含'index'的元单元格的Eigen :: Array3i(meta)索引)
Eigen::Array3i GetMetaIndex(const Eigen::Array3i& index) const {
DCHECK((index >= 0).all()) << index;
const Eigen::Array3i meta_index = index / WrappedGrid::grid_size();
DCHECK((meta_index < (1 << bits_)).all()) << index;
return meta_index;
}
// Grows this grid by a factor of 2 in each of the 3 dimensions.
// 在3个维度的每个维度中将此网格增长2倍
void Grow() {
const int new_bits = bits_ + 1;
CHECK_LE(new_bits, 8);
std::vector<std::unique_ptr<WrappedGrid>> new_meta_cells_(
8 * meta_cells_.size());
for (int z = 0; z != (1 << bits_); ++z) {
for (int y = 0; y != (1 << bits_); ++y) {
for (int x = 0; x != (1 << bits_); ++x) {
const Eigen::Array3i original_meta_index(x, y, z);
const Eigen::Array3i new_meta_index =
original_meta_index + (1 << (bits_ - 1));
new_meta_cells_[ToFlatIndex(new_meta_index, new_bits)] =
std::move(meta_cells_[ToFlatIndex(original_meta_index, bits_)]);
}
}
}
meta_cells_ = std::move(new_meta_cells_);
bits_ = new_bits;
}
int bits_;
std::vector<std::unique_ptr<WrappedGrid>> meta_cells_;
};
4.8 NestedGrid
NestedGrid有
2
k
B
i
t
s
∗
2
k
B
i
t
s
∗
2
k
B
i
t
s
2^{kBits}*2^{kBits}*2^{kBits}
2kBits∗2kBits∗2kBits个WrappedGrid类型的格子
每个WrappedGrid类型的格子在通过函数mutable_value()访问时创建
// A grid consisting of '2^kBits' x '2^kBits' x '2^kBits' grids of type
// 'WrappedGrid'. Wrapped grids are constructed on first access via
// 'mutable_value()'.
template <typename WrappedGrid, int kBits>
class NestedGrid {
public:
using ValueType = typename WrappedGrid::ValueType;
// Returns the number of voxels per dimension.
static int grid_size() { return WrappedGrid::grid_size() << kBits; }
// Returns the value stored at 'index', each dimension of 'index' being
// between 0 and grid_size() - 1.
ValueType value(const Eigen::Array3i& index) const {
const Eigen::Array3i meta_index = GetMetaIndex(index);
const WrappedGrid* const meta_cell =
meta_cells_[ToFlatIndex(meta_index, kBits)].get();
if (meta_cell == nullptr) {
return ValueType();
}
const Eigen::Array3i inner_index =
index - meta_index * WrappedGrid::grid_size();
return meta_cell->value(inner_index);
}
// Returns a pointer to the value at 'index' to allow changing it. If
// necessary a new wrapped grid is constructed to contain that value.
ValueType* mutable_value(const Eigen::Array3i& index) {
const Eigen::Array3i meta_index = GetMetaIndex(index);
std::unique_ptr<WrappedGrid>& meta_cell =
meta_cells_[ToFlatIndex(meta_index, kBits)];
if (meta_cell == nullptr) {
meta_cell = common::make_unique<WrappedGrid>();
}
const Eigen::Array3i inner_index =
index - meta_index * WrappedGrid::grid_size();
return meta_cell->mutable_value(inner_index);
}
// An iterator for iterating over all values not comparing equal to the
// default constructed value.
class Iterator {
public:
Iterator() : current_(nullptr), end_(nullptr), nested_iterator_() {}
explicit Iterator(const NestedGrid& nested_grid)
: current_(nested_grid.meta_cells_.data()),
end_(nested_grid.meta_cells_.data() + nested_grid.meta_cells_.size()),
nested_iterator_() {
AdvanceToValidNestedIterator();
}
void Next() {
DCHECK(!Done());
nested_iterator_.Next();
if (!nested_iterator_.Done()) {
return;
}
++current_;
AdvanceToValidNestedIterator();
}
bool Done() const { return current_ == end_; }
Eigen::Array3i GetCellIndex() const {
DCHECK(!Done());
const int index = (1 << (3 * kBits)) - (end_ - current_);
return To3DIndex(index, kBits) * WrappedGrid::grid_size() +
nested_iterator_.GetCellIndex();
}
const ValueType& GetValue() const {
DCHECK(!Done());
return nested_iterator_.GetValue();
}
private:
void AdvanceToValidNestedIterator() {
for (; !Done(); ++current_) {
if (*current_ != nullptr) {
nested_iterator_ = typename WrappedGrid::Iterator(**current_);
if (!nested_iterator_.Done()) {
break;
}
}
}
}
const std::unique_ptr<WrappedGrid>* current_;
const std::unique_ptr<WrappedGrid>* end_;
typename WrappedGrid::Iterator nested_iterator_;
};
private:
// Returns the Eigen::Array3i (meta) index of the meta cell containing
// 'index'.
Eigen::Array3i GetMetaIndex(const Eigen::Array3i& index) const {
DCHECK((index >= 0).all()) << index;
const Eigen::Array3i meta_index = index / WrappedGrid::grid_size();
DCHECK((meta_index < (1 << kBits)).all()) << index;
return meta_index;
}
std::array<std::unique_ptr<WrappedGrid>, 1 << (3 * kBits)> meta_cells_;
};
4.9 FlatGrid
FlatGrid是一个包含
2
k
B
i
t
s
∗
2
k
B
i
t
s
∗
2
k
B
i
t
s
2^{kBits}*2^{kBits}*2^{kBits}
2kBits∗2kBits∗2kBits个存储ValueType值的连续内存块
每一维的索引从0开始
// A flat grid of '2^kBits' x '2^kBits' x '2^kBits' voxels storing values of
// type 'ValueType' in contiguous memory. Indices in each dimension are 0-based.
template <typename TValueType, int kBits>
class FlatGrid {
public:
using ValueType = TValueType;
// Creates a new flat grid with all values being default constructed.
FlatGrid() {
for (ValueType& value : cells_) {
value = ValueType();
}
}
FlatGrid(const FlatGrid&) = delete;
FlatGrid& operator=(const FlatGrid&) = delete;
// Returns the number of voxels per dimension.
static int grid_size() { return 1 << kBits; }
// Returns the value stored at 'index', each dimension of 'index' being
// between 0 and grid_size() - 1.
ValueType value(const Eigen::Array3i& index) const {
return cells_[ToFlatIndex(index, kBits)];
}
// Returns a pointer to a value to allow changing it.
ValueType* mutable_value(const Eigen::Array3i& index) {
return &cells_[ToFlatIndex(index, kBits)];
}
// An iterator for iterating over all values not comparing equal to the
// default constructed value.
class Iterator {
public:
Iterator() : current_(nullptr), end_(nullptr) {}
explicit Iterator(const FlatGrid& flat_grid)
: current_(flat_grid.cells_.data()),
end_(flat_grid.cells_.data() + flat_grid.cells_.size()) {
while (!Done() && IsDefaultValue(*current_)) {
++current_;
}
}
void Next() {
DCHECK(!Done());
do {
++current_;
} while (!Done() && IsDefaultValue(*current_));
}
bool Done() const { return current_ == end_; }
Eigen::Array3i GetCellIndex() const {
DCHECK(!Done());
const int index = (1 << (3 * kBits)) - (end_ - current_);
return To3DIndex(index, kBits);
}
const ValueType& GetValue() const {
DCHECK(!Done());
return *current_;
}
private:
const ValueType* current_;
const ValueType* end_;
};
private:
std::array<ValueType, 1 << (3 * kBits)> cells_;
};