GridInterface基本废弃
namespace cartographer {
namespace mapping {
class GridInterface {
// todo(kdaun) move mutual functions of Grid2D/3D here
public:
virtual ~GridInterface() {}
};
} // namespace mapping
} // namespace cartographer
Grid2d类
class Grid2D : public GridInterface {
public:
Grid2D(const MapLimits& limits, float min_correspondence_cost,
float max_correspondence_cost,
ValueConversionTables* conversion_tables);
explict Grid2D(const proto::Grid2D& proto,
ValueConversionTables* conversion_tables);
const mapLimits& limits() const { return limits_;}
void FinsihUpdate();
float GetCorrespondenceCost(const Eigen::Aray2i& cell_index) const{}
virtual GridType GetGridType() const = 0;
float GetMinCorrespondenceCost() const {}
float GetMaxCorrespondenceCost() const { return max_correspondence_cost_;};
bool IsKnown(const Eigen::Array2i& cell_index) const {}
void ComputeCroppedLimits(Eigen::Array2i* const offset,
CellLimits* const limits) const;
virtual void GrowLimits(const Eigen::vector2f& point);
virtual std::unique_ptr<Grid2D> ComputeCroppedGrid() const = 0;
virtual proto::Grid2D ToProto() const;
virtual bool DraeToSubmapTexture(
proto::SubmapQuery::Response::SubmapTexture* const texture, transform::Rigid3d local_pose) const = 0;
protectef:
void GrowLimits(const Eigen::Vector2f& point,
const std::vector<std::vector<uint16>*>& grids,
const std::vector<uint16>& grids_unknown_cell_values);
const std::vector<uint16>& correspondence_cost_cells(){
return corresopondence_cost_cells;
}
const std::vector<int>& update_indices() const { return update_indices_;}
const Eigen::AlignedBox2i& known_cells_box() const{
return known_cell_box_;
}
std::vector<uint16>* mutable_correspondence_cost_cells() {
return &correspondence_cost_cells_;
}
std::vector<int>* 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 {
CHECK(limits_.Contains(cell_index)) << cell_index;
return limits_.cell_limits().num_x_cells * cell_index.y() + cell_index.x();
}
private:
MapLimits limits_;
std::vector<unit16> correspondence_cost_cells_;
float min_correspondence_cost;
float max_correspondence_cost;
std::vector<int> update_indices_;
Eigen::AlignedBox2i known_cell_box_;
const std::vector<float>* value_to_correspondence_cost_table_;
}
这里面最重要的就是GrowLimits
它是如何扩展更新地图的呢?
细节见代码,效果如下图
在坐标系不变的情况下,向四周扩展了地图,扩展距离为原来地图的一半
对应的实现代码如下:
void Grid2D::GrowLimits(const Eigen::Vector2f& point,
const std::vector<std::vector<uint16>*>& grids,
const std::vector<uint16>& grids_unknown_cell_values) {
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;
for (size_t grid_index = 0; grid_index < grids.size(); ++grid_index) {
std::vector<uint16> new_cells(new_size,
grids_unknown_cell_values[grid_index]);
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] =
(*grids[grid_index])[j + i * limits_.cell_limits().num_x_cells];
}
}
*grids[grid_index] = new_cells;
}
limits_ = new_limits;
if (!known_cells_box_.isEmpty()) {
known_cells_box_.translate(Eigen::Vector2i(x_offset, y_offset));
}
}
}