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

17
cartographer_ros/cartographer_ros/assets_writer.cc
View File

@ -67,16 +67,19 @@ 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,
Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitY())), carto::transform::Rigid3f::Rotation(
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,
Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ())), carto::transform::Rigid3f::Rotation(
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,
Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitZ())), carto::transform::Rigid3f::Rotation(
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(
file_writer_factory(stem + ".ply"), &xz_xray_points_processor); file_writer_factory(stem + ".ply"), &xz_xray_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();

11
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,11 +72,10 @@ 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);
} }
} }
CHECK(latest_time != carto::common::Time::min()); CHECK(latest_time != carto::common::Time::min());

9
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(),
{Eigen::Vector3f(-30.f, 1.f, 0.f),
Eigen::Vector3f(50.f, -10.f, 0.f)},
{}},
::cartographer::sensor::Compress(::cartographer::sensor::RangeData{ ::cartographer::sensor::Compress(::cartographer::sensor::RangeData{
Eigen::Vector3f::Zero(), {}, {}}), Eigen::Vector3f::Zero(),
{Eigen::Vector3f(-30.f, 1.f, 0.f),
Eigen::Vector3f(50.f, -10.f, 0.f)},
{}}),
::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;