diff --git a/cartographer_ros/cartographer_ros/assets_writer.cc b/cartographer_ros/cartographer_ros/assets_writer.cc index ffb700d..0d4d9ad 100644 --- a/cartographer_ros/cartographer_ros/assets_writer.cc +++ b/cartographer_ros/cartographer_ros/assets_writer.cc @@ -67,16 +67,19 @@ void Write3DAssets( carto::io::NullPointsProcessor null_points_processor; carto::io::XRayPointsProcessor xy_xray_points_processor( - voxel_size, carto::transform::Rigid3f::Rotation( - Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitY())), + voxel_size, + carto::transform::Rigid3f::Rotation( + Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitY())), {}, stem + "_xray_xy", file_writer_factory, &null_points_processor); carto::io::XRayPointsProcessor yz_xray_points_processor( - voxel_size, carto::transform::Rigid3f::Rotation( - Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ())), + voxel_size, + carto::transform::Rigid3f::Rotation( + Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ())), {}, stem + "_xray_yz", file_writer_factory, &xy_xray_points_processor); carto::io::XRayPointsProcessor xz_xray_points_processor( - voxel_size, carto::transform::Rigid3f::Rotation( - Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitZ())), + voxel_size, + carto::transform::Rigid3f::Rotation( + Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitZ())), {}, stem + "_xray_xz", file_writer_factory, &yz_xray_points_processor); carto::io::PlyWritingPointsProcessor ply_writing_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]) { const carto::sensor::RangeData range_data = carto::sensor::TransformRangeData( - carto::sensor::Decompress(node.constant_data->range_data_3d), + carto::sensor::Decompress(node.constant_data->range_data), node.pose.cast()); auto points_batch = carto::common::make_unique(); points_batch->time = node.time(); diff --git a/cartographer_ros/cartographer_ros/occupancy_grid.cc b/cartographer_ros/cartographer_ros/occupancy_grid.cc index 8d6fc53..00d8513 100644 --- a/cartographer_ros/cartographer_ros/occupancy_grid.cc +++ b/cartographer_ros/cartographer_ros/occupancy_grid.cc @@ -36,7 +36,7 @@ Eigen::AlignedBox2f ComputeMapBoundingBox2D( const auto& data = *node.constant_data; ::cartographer::sensor::RangeData range_data; range_data = ::cartographer::sensor::TransformRangeData( - data.range_data_2d, node.pose.cast()); + Decompress(data.range_data), node.pose.cast()); bounding_box.extend(range_data.origin.head<2>()); for (const Eigen::Vector3f& hit : range_data.returns) { bounding_box.extend(hit.head<2>()); @@ -72,11 +72,10 @@ void BuildOccupancyGrid2D( continue; } 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()), - &probability_grid); + range_data_inserter.Insert(carto::sensor::TransformRangeData( + Decompress(node.constant_data->range_data), + node.pose.cast()), + &probability_grid); } } CHECK(latest_time != carto::common::Time::min()); diff --git a/cartographer_ros/cartographer_ros/occupancy_grid_test.cc b/cartographer_ros/cartographer_ros/occupancy_grid_test.cc index 665ae77..70bc104 100644 --- a/cartographer_ros/cartographer_ros/occupancy_grid_test.cc +++ b/cartographer_ros/cartographer_ros/occupancy_grid_test.cc @@ -31,12 +31,11 @@ TEST(OccupancyGridTest, ComputeMapLimits) { const TrajectoryNode trajectory_node{ std::make_shared(TrajectoryNode::Data{ ::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{ - 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()}; constexpr double kResolution = 0.05;