看的是0.1.0的版本
std::unique_ptr<nav_msgs::OccupancyGrid>
MapBuilderBridge::BuildOccupancyGrid() {
CHECK(options_.map_builder_options.use_trajectory_builder_2d())
<< "Publishing OccupancyGrids for 3D data is not yet supported";
std::vector<::cartographer::mapping::TrajectoryNode> trajectory_nodes;
for (const auto& single_trajectory :
map_builder_.sparse_pose_graph()->GetTrajectoryNodes()) {
trajectory_nodes.insert(trajectory_nodes.end(), single_trajectory.begin(),
single_trajectory.end());
}
std::unique_ptr<nav_msgs::OccupancyGrid> occupancy_grid;
if (!trajectory_nodes.empty()) {
occupancy_grid =
cartographer::common::make_unique<nav_msgs::OccupancyGrid>();
BuildOccupancyGrid2D(
trajectory_nodes, options_.map_frame,
options_.trajectory_builder_options.trajectory_builder_2d_options()
.submaps_options(),
occupancy_grid.get());
}
return occupancy_grid;
}
// Writes an occupancy grid. 这里调接口
void Write2DAssets(
const std::vector<::cartographer::mapping::TrajectoryNode>&
trajectory_nodes,
const string& map_frame,
const ::cartographer::mapping_2d::proto::SubmapsOptions& submaps_options,
const std::string& stem) {
WriteTrajectory(trajectory_nodes, stem);
::nav_msgs::OccupancyGrid occupancy_grid;
BuildOccupancyGrid2D(trajectory_nodes, map_frame, submaps_options,
&occupancy_grid);
WriteOccupancyGridToPgmAndYaml(occupancy_grid, stem);
}
BuildOccupancyGrid2D 这个这occupancy.h下
void BuildOccupancyGrid2D(
const std::vector<::cartographer::mapping::TrajectoryNode>&
trajectory_nodes,
const string& map_frame,
const ::cartographer::mapping_2d::proto::SubmapsOptions& submaps_options,
::nav_msgs::OccupancyGrid* const occupancy_grid);
}
可以看出来 要轨迹的节点，和submap_options 应该是
//下面是轨迹的节点 结构如下，时间，激光，id, pose刚体变换
struct TrajectoryNode {
struct ConstantData {
common::Time time;
// Range data in 'pose' frame. Only used in the 2D case.
sensor::RangeData range_data_2d;
// Range data in 'pose' frame. Only used in the 3D case.
sensor::CompressedRangeData range_data_3d;
// Trajectory this node belongs to.
int trajectory_id;
// Transform from the 3D 'tracking' frame to the 'pose' frame of the range
// data, which contains roll, pitch and height for 2D. In 3D this is always
// identity.
transform::Rigid3d tracking_to_pose;
};
//下面是proto文件 就是激光insert
syntax = "proto2";
import "cartographer/mapping_2d/proto/range_data_inserter_options.proto";
package cartographer.mapping_2d.proto;
message SubmapsOptions {
// Resolution of the map in meters.
optional double resolution = 1;
// Half the width/height of each submap, its "radius".
optional double half_length = 2;
// Number of scans before adding a new submap. Each submap will get twice the
// number of scans inserted: First for initialization without being matched
// against, then while being matched.
optional int32 num_range_data = 3;
// If enabled, submap%d.png images are written for debugging.
optional bool output_debug_images = 4;
optional RangeDataInserterOptions range_data_inserter_options = 5;
}
////////////////// 激光insert
syntax = "proto2";
package cartographer.mapping_2d.proto;
message RangeDataInserterOptions {
// Probability change for a hit (this will be converted to odds and therefore
// must be greater than 0.5).
optional double hit_probability = 1;
// Probability change for a miss (this will be converted to odds and therefore
// must be less than 0.5).
optional double miss_probability = 2;
// If 'false', free space will not change the probabilities in the occupancy
// grid.
optional bool insert_free_space = 3;
}
/// 主要是submap
void Submaps::InsertRangeData(const sensor::RangeData& range_data) {
for (const int index : insertion_indices()) {
Submap* submap = submaps_[index].get();
CHECK(submap->finished_probability_grid == nullptr);
range_data_inserter_.Insert(range_data, &submap->probability_grid);
++submap->num_range_data;
}
const Submap* const last_submap = Get(size() - 1);
if (last_submap->num_range_data == options_.num_range_data()) {
AddSubmap(range_data.origin.head<2>());
}
}
//// 有个range_data_inserter 的工人 里面method
void Insert(const sensor::RangeData& range_data,
ProbabilityGrid* probability_grid) const;
///
void RangeDataInserter::Insert(const sensor::RangeData& range_data,
ProbabilityGrid* const probability_grid) const {
CHECK_NOTNULL(probability_grid)->StartUpdate();
// By not starting a new update after hits are inserted, we give hits priority
// (i.e. no hits will be ignored because of a miss in the same cell).
CastRays(range_data, probability_grid->limits(),
[this, &probability_grid](const Eigen::Array2i& hit) {
probability_grid->ApplyLookupTable(hit, hit_table_);
},
[this, &probability_grid](const Eigen::Array2i& miss) {
if (options_.insert_free_space()) {
probability_grid->ApplyLookupTable(miss, miss_table_);
}
});
}
/// 有何ray cast 的全局函数
namespace cartographer {
namespace mapping_2d {
// For each ray in 'range_data', calls 'hit_visitor' and 'miss_visitor' on the
// appropriate cells. Hits are handled before misses.
void CastRays(const sensor::RangeData& range_data, const MapLimits& limits,
const std::function<void(const Eigen::Array2i&)>& hit_visitor,
const std::function<void(const Eigen::Array2i&)>& miss_visitor);
} // namespace mapping_2d
}
// 下面就是核心部分了 有两个 一根是castray 一个是 casting rays
void CastRay(const Eigen::Array2i& begin, const Eigen::Array2i& end,
const std::function<void(const Eigen::Array2i&)>& visitor) {
// For simplicity, we order 'begin' and 'end' by their x coordinate.
if (begin.x() > end.x()) {
CastRay(end, begin, visitor);
return;
}
CHECK_GE(begin.x(), 0);
CHECK_GE(begin.y(), 0);
CHECK_GE(end.y(), 0);
// Special case: We have to draw a vertical line in full pixels, as 'begin'
// and 'end' have the same full pixel x coordinate.
if (begin.x() / kSubpixelScale == end.x() / kSubpixelScale) {
Eigen::Array2i current(begin.x() / kSubpixelScale,
std::min(begin.y(), end.y()) / kSubpixelScale);
const int end_y = std::max(begin.y(), end.y()) / kSubpixelScale;
for (; current.y() <= end_y; ++current.y()) {
visitor(current);
}
return;
}
const int64 dx = end.x() - begin.x();
const int64 dy = end.y() - begin.y();
const int64 denominator = 2 * kSubpixelScale * dx;
// The current full pixel coordinates. We begin at 'begin'.
Eigen::Array2i current = begin / kSubpixelScale;
// To represent subpixel centers, we use a factor of 2 * 'kSubpixelScale' in
// the denominator.
// +-+-+-+ -- 1 = (2 * kSubpixelScale) / (2 * kSubpixelScale)
// | | | |
// +-+-+-+
// | | | |
// +-+-+-+ -- top edge of first subpixel = 2 / (2 * kSubpixelScale)
// | | | | -- center of first subpixel = 1 / (2 * kSubpixelScale)
// +-+-+-+ -- 0 = 0 / (2 * kSubpixelScale)
// The center of the subpixel part of 'begin.y()' assuming the
// 'denominator', i.e., sub_y / denominator is in (0, 1).
int64 sub_y = (2 * (begin.y() % kSubpixelScale) + 1) * dx;
// The distance from the from 'begin' to the right pixel border, to be divided
// by 2 * 'kSubpixelScale'.
const int first_pixel =
2 * kSubpixelScale - 2 * (begin.x() % kSubpixelScale) - 1;
// The same from the left pixel border to 'end'.
const int last_pixel = 2 * (end.x() % kSubpixelScale) + 1;
// The full pixel x coordinate of 'end'.
const int end_x = std::max(begin.x(), end.x()) / kSubpixelScale;
// Move from 'begin' to the next pixel border to the right.
sub_y += dy * first_pixel;
if (dy > 0) {
while (true) {
visitor(current);
while (sub_y > denominator) {
sub_y -= denominator;
++current.y();
visitor(current);
}
++current.x();
if (sub_y == denominator) {
sub_y -= denominator;
++current.y();
}
if (current.x() == end_x) {
break;
}
// Move from one pixel border to the next.
sub_y += dy * 2 * kSubpixelScale;
}
// Move from the pixel border on the right to 'end'.
sub_y += dy * last_pixel;
visitor(current);
while (sub_y > denominator) {
sub_y -= denominator;
++current.y();
visitor(current);
}
CHECK_NE(sub_y, denominator);
CHECK_EQ(current.y(), end.y() / kSubpixelScale);
return;
}
// Same for lines non-ascending in y coordinates.
while (true) {
visitor(current);
while (sub_y < 0) {
sub_y += denominator;
--current.y();
visitor(current);
}
++current.x();
if (sub_y == 0) {
sub_y += denominator;
--current.y();
}
if (current.x() == end_x) {
break;
}
sub_y += dy * 2 * kSubpixelScale;
}
sub_y += dy * last_pixel;
visitor(current);
while (sub_y < 0) {
sub_y += denominator;
--current.y();
visitor(current);
}
CHECK_NE(sub_y, 0);
CHECK_EQ(current.y(), end.y() / kSubpixelScale);
}
} // namespace
void CastRays(const sensor::RangeData& range_data, const MapLimits& limits,
const std::function<void(const Eigen::Array2i&)>& hit_visitor,
const std::function<void(const Eigen::Array2i&)>& miss_visitor) {
const double superscaled_resolution = limits.resolution() / kSubpixelScale;
const MapLimits superscaled_limits(
superscaled_resolution, limits.max(),
CellLimits(limits.cell_limits().num_x_cells * kSubpixelScale,
limits.cell_limits().num_y_cells * kSubpixelScale));
const Eigen::Array2i begin =
superscaled_limits.GetXYIndexOfCellContainingPoint(range_data.origin.x(),
range_data.origin.y());
// Compute and add the end points.
std::vector<Eigen::Array2i> ends;
ends.reserve(range_data.returns.size());
for (const Eigen::Vector3f& hit : range_data.returns) {
ends.push_back(
superscaled_limits.GetXYIndexOfCellContainingPoint(hit.x(), hit.y()));
hit_visitor(ends.back() / kSubpixelScale);
}
// Now add the misses.
for (const Eigen::Array2i& end : ends) {
CastRay(begin, end, miss_visitor);
}
// Finally, compute and add empty rays based on misses in the scan.
for (const Eigen::Vector3f& missing_echo : range_data.misses) {
CastRay(begin,
superscaled_limits.GetXYIndexOfCellContainingPoint(
missing_echo.x(), missing_echo.y()),
miss_visitor);
}
}
} // namespace mapping_2d
} // namespace cartographer