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

Follow googlecartographer/cartographer#396. (#417)

master
Wolfgang Hess 2017-07-07 17:46:59 +02:00 committed by GitHub
parent 6b1e9fb55c
commit 35389d0adb
3 changed files with 19 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
cartographer_ros/cartographer_ros/assets_writer.cc
View File

@ -67,15 +67,18 @@ void Write3DAssets(
carto::io::NullPointsProcessor null_points_processor; carto::io::NullPointsProcessor null_points_processor;
carto::io::XRayPointsProcessor xy_xray_points_processor( carto::io::XRayPointsProcessor xy_xray_points_processor(
voxel_size, carto::transform::Rigid3f::Rotation( voxel_size,
carto::transform::Rigid3f::Rotation(
Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitY())), Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitY())),
{}, stem + "_xray_xy", file_writer_factory, &null_points_processor); {}, stem + "_xray_xy", file_writer_factory, &null_points_processor);
carto::io::XRayPointsProcessor yz_xray_points_processor( carto::io::XRayPointsProcessor yz_xray_points_processor(
voxel_size, carto::transform::Rigid3f::Rotation( voxel_size,
carto::transform::Rigid3f::Rotation(
Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ())), Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ())),
{}, stem + "_xray_yz", file_writer_factory, &xy_xray_points_processor); {}, stem + "_xray_yz", file_writer_factory, &xy_xray_points_processor);
carto::io::XRayPointsProcessor xz_xray_points_processor( carto::io::XRayPointsProcessor xz_xray_points_processor(
voxel_size, carto::transform::Rigid3f::Rotation( voxel_size,
carto::transform::Rigid3f::Rotation(
Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitZ())), Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitZ())),
{}, stem + "_xray_xz", file_writer_factory, &yz_xray_points_processor); {}, stem + "_xray_xz", file_writer_factory, &yz_xray_points_processor);
carto::io::PlyWritingPointsProcessor ply_writing_points_processor( carto::io::PlyWritingPointsProcessor ply_writing_points_processor(
@ -86,7 +89,7 @@ void Write3DAssets(
for (const auto& node : all_trajectory_nodes[trajectory_id]) { for (const auto& node : all_trajectory_nodes[trajectory_id]) {
const carto::sensor::RangeData range_data = const carto::sensor::RangeData range_data =
carto::sensor::TransformRangeData( carto::sensor::TransformRangeData(
carto::sensor::Decompress(node.constant_data->range_data_3d), carto::sensor::Decompress(node.constant_data->range_data),
node.pose.cast<float>()); node.pose.cast<float>());
auto points_batch = carto::common::make_unique<carto::io::PointsBatch>(); auto points_batch = carto::common::make_unique<carto::io::PointsBatch>();
points_batch->time = node.time(); points_batch->time = node.time();

7
cartographer_ros/cartographer_ros/occupancy_grid.cc
View File

@ -36,7 +36,7 @@ Eigen::AlignedBox2f ComputeMapBoundingBox2D(
const auto& data = *node.constant_data; const auto& data = *node.constant_data;
::cartographer::sensor::RangeData range_data; ::cartographer::sensor::RangeData range_data;
range_data = ::cartographer::sensor::TransformRangeData( range_data = ::cartographer::sensor::TransformRangeData(
data.range_data_2d, node.pose.cast<float>()); Decompress(data.range_data), node.pose.cast<float>());
bounding_box.extend(range_data.origin.head<2>()); bounding_box.extend(range_data.origin.head<2>());
for (const Eigen::Vector3f& hit : range_data.returns) { for (const Eigen::Vector3f& hit : range_data.returns) {
bounding_box.extend(hit.head<2>()); bounding_box.extend(hit.head<2>());
@ -72,9 +72,8 @@ void BuildOccupancyGrid2D(
continue; continue;
} }
latest_time = std::max(latest_time, node.time()); latest_time = std::max(latest_time, node.time());
CHECK(node.constant_data->range_data_3d.returns.empty()); range_data_inserter.Insert(carto::sensor::TransformRangeData(
range_data_inserter.Insert( Decompress(node.constant_data->range_data),
carto::sensor::TransformRangeData(node.constant_data->range_data_2d,
node.pose.cast<float>()), node.pose.cast<float>()),
&probability_grid); &probability_grid);
} }

7
cartographer_ros/cartographer_ros/occupancy_grid_test.cc
View File

@ -31,12 +31,11 @@ TEST(OccupancyGridTest, ComputeMapLimits) {
const TrajectoryNode trajectory_node{ const TrajectoryNode trajectory_node{
std::make_shared<TrajectoryNode::Data>(TrajectoryNode::Data{ std::make_shared<TrajectoryNode::Data>(TrajectoryNode::Data{
::cartographer::common::FromUniversal(52), ::cartographer::common::FromUniversal(52),
::cartographer::sensor::RangeData{Eigen::Vector3f::Zero(), ::cartographer::sensor::Compress(::cartographer::sensor::RangeData{
Eigen::Vector3f::Zero(),
{Eigen::Vector3f(-30.f, 1.f, 0.f), {Eigen::Vector3f(-30.f, 1.f, 0.f),
Eigen::Vector3f(50.f, -10.f, 0.f)}, Eigen::Vector3f(50.f, -10.f, 0.f)},
{}}, {}}),
::cartographer::sensor::Compress(::cartographer::sensor::RangeData{
Eigen::Vector3f::Zero(), {}, {}}),
::cartographer::transform::Rigid3d::Identity()}), ::cartographer::transform::Rigid3d::Identity()}),
::cartographer::transform::Rigid3d::Identity()}; ::cartographer::transform::Rigid3d::Identity()};
constexpr double kResolution = 0.05; constexpr double kResolution = 0.05;