12345qiupeng
/
cartographer_ros
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Make WriteAssets multi trajectory aware. (#394) 

Related to #343.
master
Holger Rapp 2017-06-23 18:16:32 +02:00 committed by GitHub
parent 08ff3f9c42
commit 04df159d82
6 changed files with 122 additions and 101 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
cartographer_ros/cartographer_ros/assets_writer.cc
View File

@ -31,23 +31,34 @@
namespace cartographer_ros {
// Writes an occupancy grid.
bool HasNonTrimmedNode(
const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
all_trajectory_nodes) {
for (const auto& trajectory_nodes : all_trajectory_nodes) {
for (const auto& node : trajectory_nodes) {
if (!node.trimmed()) {
return true;
}
}
}
return false;
}
void Write2DAssets(
const std::vector<::cartographer::mapping::TrajectoryNode>&
trajectory_nodes,
const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
all_trajectory_nodes,
const string& map_frame,
const ::cartographer::mapping_2d::proto::SubmapsOptions& submaps_options,
const std::string& stem) {
::nav_msgs::OccupancyGrid occupancy_grid;
BuildOccupancyGrid2D(trajectory_nodes, map_frame, submaps_options,
BuildOccupancyGrid2D(all_trajectory_nodes, map_frame, submaps_options,
&occupancy_grid);
WriteOccupancyGridToPgmAndYaml(occupancy_grid, stem);
}
// Writes X-ray images and PLY files from the 'trajectory_nodes'. The filenames
// will all start with 'stem'.
void Write3DAssets(const std::vector<::cartographer::mapping::TrajectoryNode>&
trajectory_nodes,
void Write3DAssets(
const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
all_trajectory_nodes,
const double voxel_size, const std::string& stem) {
namespace carto = ::cartographer;
const auto file_writer_factory = [](const string& filename) {
@ -73,17 +84,21 @@ void Write3DAssets(const std::vector<::cartographer::mapping::TrajectoryNode>&
carto::io::PlyWritingPointsProcessor ply_writing_points_processor(
file_writer_factory(stem + ".ply"), &xz_xray_points_processor);
for (const auto& node : trajectory_nodes) {
for (size_t trajectory_id = 0; trajectory_id < all_trajectory_nodes.size();
++trajectory_id) {
for (const auto& node : all_trajectory_nodes[trajectory_id]) {
const carto::sensor::RangeData range_data =
carto::sensor::TransformRangeData(
carto::sensor::Decompress(node.constant_data->range_data_3d),
node.pose.cast<float>());
auto points_batch = carto::common::make_unique<carto::io::PointsBatch>();
points_batch->time = node.time();
points_batch->origin = range_data.origin;
points_batch->trajectory_index = trajectory_id;
points_batch->points = range_data.returns;
ply_writing_points_processor.Process(std::move(points_batch));
}
}
ply_writing_points_processor.Flush();
}

17
cartographer_ros/cartographer_ros/assets_writer.h
View File

@ -25,18 +25,25 @@
namespace cartographer_ros {
// Returns 'true' if there is at least one untrimmed node for any trajectory.
// The Write?DAssets functions expects this to be 'true'.
bool HasNonTrimmedNode(
const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
all_trajectory_nodes);
// Writes a trajectory proto and an occupancy grid.
void Write2DAssets(
const std::vector<::cartographer::mapping::TrajectoryNode>&
trajectory_nodes,
const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
all_trajectory_nodes,
const string& map_frame,
const ::cartographer::mapping_2d::proto::SubmapsOptions& submaps_options,
const std::string& stem);
// Writes X-ray images, trajectory proto, and PLY files from the
// 'trajectory_nodes'. The filenames will all start with 'stem'.
void Write3DAssets(const std::vector<::cartographer::mapping::TrajectoryNode>&
trajectory_nodes,
// 'all_trajectory_nodes'. The filenames will all start with 'stem'.
void Write3DAssets(
const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
all_trajectory_nodes,
const double voxel_size, const std::string& stem);
} // namespace cartographer_ros

34
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

@ -38,6 +38,7 @@ int MapBuilderBridge::AddTrajectory(
expected_sensor_ids, trajectory_options.trajectory_builder_options);
LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'.";
// Make sure there is no trajectory with 'trajectory_id' yet.
CHECK_EQ(sensor_bridges_.count(trajectory_id), 0);
sensor_bridges_[trajectory_id] =
cartographer::common::make_unique<SensorBridge>(
@ -53,6 +54,7 @@ int MapBuilderBridge::AddTrajectory(
void MapBuilderBridge::FinishTrajectory(const int trajectory_id) {
LOG(INFO) << "Finishing trajectory with ID '" << trajectory_id << "'...";
// Make sure there is a trajectory with 'trajectory_id'.
CHECK_EQ(sensor_bridges_.count(trajectory_id), 1);
map_builder_.FinishTrajectory(trajectory_id);
map_builder_.sparse_pose_graph()->RunFinalOptimization();
@ -72,16 +74,18 @@ void MapBuilderBridge::SerializeState(const std::string& stem) {
void MapBuilderBridge::WriteAssets(const string& stem) {
const auto all_trajectory_nodes =
map_builder_.sparse_pose_graph()->GetTrajectoryNodes();
// TODO(yutakaoka): Add multi-trajectory support.
CHECK_EQ(trajectory_options_.count(0), 1);
CHECK_EQ(all_trajectory_nodes.size(), 1);
if (all_trajectory_nodes[0].empty()) {
if (!HasNonTrimmedNode(all_trajectory_nodes)) {
LOG(WARNING) << "No data was collected and no assets will be written.";
} else {
return;
}
// Make sure there is a trajectory with id = 0.
CHECK_EQ(trajectory_options_.count(0), 1);
LOG(INFO) << "Writing assets with stem '" << stem << "'...";
if (node_options_.map_builder_options.use_trajectory_builder_2d()) {
// We arbitrarily use the submap_options() from the first trajectory to
// write the 2D assets.
Write2DAssets(
all_trajectory_nodes[0], node_options_.map_frame,
all_trajectory_nodes, node_options_.map_frame,
trajectory_options_[0]
.trajectory_builder_options.trajectory_builder_2d_options()
.submaps_options(),
@ -90,7 +94,7 @@ void MapBuilderBridge::WriteAssets(const string& stem) {
if (node_options_.map_builder_options.use_trajectory_builder_3d()) {
Write3DAssets(
all_trajectory_nodes[0],
all_trajectory_nodes,
trajectory_options_[0]
.trajectory_builder_options.trajectory_builder_3d_options()
.submaps_options()
@ -98,7 +102,6 @@ void MapBuilderBridge::WriteAssets(const string& stem) {
stem);
}
}
}
bool MapBuilderBridge::HandleSubmapQuery(
cartographer_ros_msgs::SubmapQuery::Request& request,
@ -153,20 +156,16 @@ std::unique_ptr<nav_msgs::OccupancyGrid>
MapBuilderBridge::BuildOccupancyGrid() {
CHECK(node_options_.map_builder_options.use_trajectory_builder_2d())
<< "Publishing OccupancyGrids for 3D data is not yet supported";
std::vector<::cartographer::mapping::TrajectoryNode> flat_trajectory_nodes;
for (const auto& single_trajectory_nodes :
map_builder_.sparse_pose_graph()->GetTrajectoryNodes()) {
flat_trajectory_nodes.insert(flat_trajectory_nodes.end(),
single_trajectory_nodes.begin(),
single_trajectory_nodes.end());
}
const auto all_trajectory_nodes =
map_builder_.sparse_pose_graph()->GetTrajectoryNodes();
std::unique_ptr<nav_msgs::OccupancyGrid> occupancy_grid;
if (!flat_trajectory_nodes.empty()) {
if (HasNonTrimmedNode(all_trajectory_nodes)) {
occupancy_grid =
cartographer::common::make_unique<nav_msgs::OccupancyGrid>();
// Make sure there is a trajectory with id = 0.
CHECK_EQ(trajectory_options_.count(0), 1);
BuildOccupancyGrid2D(
flat_trajectory_nodes, node_options_.map_frame,
all_trajectory_nodes, node_options_.map_frame,
trajectory_options_[0]
.trajectory_builder_options.trajectory_builder_2d_options()
.submaps_options(),
@ -190,6 +189,7 @@ MapBuilderBridge::GetTrajectoryStates() {
continue;
}
// Make sure there is a trajectory with 'trajectory_id'.
CHECK_EQ(trajectory_options_.count(trajectory_id), 1);
trajectory_states[trajectory_id] = {
pose_estimate,

39
cartographer_ros/cartographer_ros/occupancy_grid.cc
View File

@ -24,24 +24,19 @@
namespace {
Eigen::AlignedBox2f ComputeMapBoundingBox(
const std::vector<::cartographer::mapping::TrajectoryNode>&
trajectory_nodes) {
Eigen::AlignedBox2f ComputeMapBoundingBox2D(
const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
all_trajectory_nodes) {
Eigen::AlignedBox2f bounding_box(Eigen::Vector2f::Zero());
for (const auto& trajectory_nodes : all_trajectory_nodes) {
for (const auto& node : trajectory_nodes) {
if (node.trimmed()) {
continue;
}
const auto& data = *node.constant_data;
::cartographer::sensor::RangeData range_data;
if (!data.range_data_3d.returns.empty()) {
range_data = ::cartographer::sensor::TransformRangeData(
::cartographer::sensor::Decompress(data.range_data_3d),
node.pose.cast<float>());
} else {
range_data = ::cartographer::sensor::TransformRangeData(
data.range_data_2d, node.pose.cast<float>());
}
bounding_box.extend(range_data.origin.head<2>());
for (const Eigen::Vector3f& hit : range_data.returns) {
bounding_box.extend(hit.head<2>());
@ -50,6 +45,7 @@ Eigen::AlignedBox2f ComputeMapBoundingBox(
bounding_box.extend(miss.head<2>());
}
}
}
return bounding_box;
}
@ -58,31 +54,33 @@ Eigen::AlignedBox2f ComputeMapBoundingBox(
namespace cartographer_ros {
void BuildOccupancyGrid2D(
const std::vector<::cartographer::mapping::TrajectoryNode>&
trajectory_nodes,
const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
all_trajectory_nodes,
const string& map_frame,
const ::cartographer::mapping_2d::proto::SubmapsOptions& submaps_options,
::nav_msgs::OccupancyGrid* const occupancy_grid) {
CHECK(!trajectory_nodes.empty());
namespace carto = ::cartographer;
const carto::mapping_2d::MapLimits map_limits =
ComputeMapLimits(submaps_options.resolution(), trajectory_nodes);
ComputeMapLimits(submaps_options.resolution(), all_trajectory_nodes);
carto::mapping_2d::ProbabilityGrid probability_grid(map_limits);
carto::mapping_2d::RangeDataInserter range_data_inserter(
submaps_options.range_data_inserter_options());
carto::common::Time latest_time = carto::common::Time::min();
for (const auto& trajectory_nodes : all_trajectory_nodes) {
for (const auto& node : trajectory_nodes) {
if (node.trimmed()) {
continue;
}
CHECK(node.constant_data->range_data_3d.returns.empty()); // No 3D yet.
latest_time = std::max(latest_time, node.time());
CHECK(node.constant_data->range_data_3d.returns.empty());
range_data_inserter.Insert(
carto::sensor::TransformRangeData(node.constant_data->range_data_2d,
node.pose.cast<float>()),
&probability_grid);
}
// TODO(whess): Compute the latest time of in 'trajectory_nodes'.
occupancy_grid->header.stamp = ToRos(trajectory_nodes.back().time());
}
CHECK(latest_time != carto::common::Time::min());
occupancy_grid->header.stamp = ToRos(latest_time);
occupancy_grid->header.frame_id = map_frame;
occupancy_grid->info.map_load_time = occupancy_grid->header.stamp;
@ -128,9 +126,10 @@ void BuildOccupancyGrid2D(
::cartographer::mapping_2d::MapLimits ComputeMapLimits(
const double resolution,
const std::vector<::cartographer::mapping::TrajectoryNode>&
trajectory_nodes) {
Eigen::AlignedBox2f bounding_box = ComputeMapBoundingBox(trajectory_nodes);
const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
all_trajectory_nodes) {
Eigen::AlignedBox2f bounding_box =
ComputeMapBoundingBox2D(all_trajectory_nodes);
// Add some padding to ensure all rays are still contained in the map after
// discretization.
const float kPadding = 3.f * resolution;

8
cartographer_ros/cartographer_ros/occupancy_grid.h
View File

@ -29,8 +29,8 @@
namespace cartographer_ros {
void BuildOccupancyGrid2D(
const std::vector<::cartographer::mapping::TrajectoryNode>&
trajectory_nodes,
const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
all_trajectory_nodes,
const string& map_frame,
const ::cartographer::mapping_2d::proto::SubmapsOptions& submaps_options,
::nav_msgs::OccupancyGrid* const occupancy_grid);
@ -39,8 +39,8 @@ void BuildOccupancyGrid2D(
// misses) in the 'trajectory_nodes'.
::cartographer::mapping_2d::MapLimits ComputeMapLimits(
double resolution,
const std::vector<::cartographer::mapping::TrajectoryNode>&
trajectory_nodes);
const std::vector<std::vector<::cartographer::mapping::TrajectoryNode>>&
all_trajectory_nodes);
} // namespace cartographer_ros

2
cartographer_ros/cartographer_ros/occupancy_grid_test.cc
View File

@ -41,7 +41,7 @@ TEST(OccupancyGridTest, ComputeMapLimits) {
::cartographer::transform::Rigid3d::Identity()};
constexpr double kResolution = 0.05;
const ::cartographer::mapping_2d::MapLimits limits =
ComputeMapLimits(kResolution, {trajectory_node});
ComputeMapLimits(kResolution, {{trajectory_node}});
constexpr float kPaddingAwareTolerance = 5 * kResolution;
EXPECT_NEAR(50.f, limits.max().x(), kPaddingAwareTolerance);
EXPECT_NEAR(1.f, limits.max().y(), kPaddingAwareTolerance);