parent
2dd30a853b
commit
8b3af77fde
|
@ -23,7 +23,7 @@
|
||||||
#include "cartographer/io/points_processor.h"
|
#include "cartographer/io/points_processor.h"
|
||||||
#include "cartographer/io/xray_points_processor.h"
|
#include "cartographer/io/xray_points_processor.h"
|
||||||
#include "cartographer/mapping/proto/trajectory.pb.h"
|
#include "cartographer/mapping/proto/trajectory.pb.h"
|
||||||
#include "cartographer/mapping_2d/proto/laser_fan_inserter_options.pb.h"
|
#include "cartographer/mapping_2d/proto/range_data_inserter_options.pb.h"
|
||||||
#include "cartographer_ros/map_writer.h"
|
#include "cartographer_ros/map_writer.h"
|
||||||
#include "cartographer_ros/occupancy_grid.h"
|
#include "cartographer_ros/occupancy_grid.h"
|
||||||
#include "nav_msgs/OccupancyGrid.h"
|
#include "nav_msgs/OccupancyGrid.h"
|
||||||
|
@ -72,13 +72,14 @@ void Write3DAssets(const std::vector<::cartographer::mapping::TrajectoryNode>&
|
||||||
file_writer_factory(stem + ".ply"), &xz_xray_points_processor);
|
file_writer_factory(stem + ".ply"), &xz_xray_points_processor);
|
||||||
|
|
||||||
for (const auto& node : trajectory_nodes) {
|
for (const auto& node : trajectory_nodes) {
|
||||||
const carto::sensor::LaserFan laser_fan = carto::sensor::TransformLaserFan(
|
const carto::sensor::RangeData range_data =
|
||||||
carto::sensor::Decompress(node.constant_data->range_data_3d),
|
carto::sensor::TransformRangeData(
|
||||||
node.pose.cast<float>());
|
carto::sensor::Decompress(node.constant_data->range_data_3d),
|
||||||
|
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->origin = laser_fan.origin;
|
points_batch->origin = range_data.origin;
|
||||||
points_batch->points = laser_fan.returns;
|
points_batch->points = range_data.returns;
|
||||||
for (const uint8 reflectivity :
|
for (const uint8 reflectivity :
|
||||||
node.constant_data->range_data_3d.reflectivities) {
|
node.constant_data->range_data_3d.reflectivities) {
|
||||||
points_batch->colors.push_back(
|
points_batch->colors.push_back(
|
||||||
|
|
|
@ -26,8 +26,8 @@
|
||||||
#include "cartographer/io/file_writer.h"
|
#include "cartographer/io/file_writer.h"
|
||||||
#include "cartographer/io/points_processor.h"
|
#include "cartographer/io/points_processor.h"
|
||||||
#include "cartographer/io/points_processor_pipeline_builder.h"
|
#include "cartographer/io/points_processor_pipeline_builder.h"
|
||||||
#include "cartographer/sensor/laser.h"
|
|
||||||
#include "cartographer/sensor/point_cloud.h"
|
#include "cartographer/sensor/point_cloud.h"
|
||||||
|
#include "cartographer/sensor/range_data.h"
|
||||||
#include "cartographer/transform/transform_interpolation_buffer.h"
|
#include "cartographer/transform/transform_interpolation_buffer.h"
|
||||||
#include "cartographer_ros/bag_reader.h"
|
#include "cartographer_ros/bag_reader.h"
|
||||||
#include "cartographer_ros/msg_conversion.h"
|
#include "cartographer_ros/msg_conversion.h"
|
||||||
|
|
|
@ -104,7 +104,7 @@ cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() {
|
||||||
++submap_index) {
|
++submap_index) {
|
||||||
cartographer_ros_msgs::SubmapEntry submap_entry;
|
cartographer_ros_msgs::SubmapEntry submap_entry;
|
||||||
submap_entry.submap_version =
|
submap_entry.submap_version =
|
||||||
submaps->Get(submap_index)->end_laser_fan_index;
|
submaps->Get(submap_index)->end_range_data_index;
|
||||||
submap_entry.pose = ToGeometryMsgPose(submap_transforms[submap_index]);
|
submap_entry.pose = ToGeometryMsgPose(submap_transforms[submap_index]);
|
||||||
trajectory_submap_list.submap.push_back(submap_entry);
|
trajectory_submap_list.submap.push_back(submap_entry);
|
||||||
}
|
}
|
||||||
|
|
|
@ -162,11 +162,11 @@ sensor_msgs::PointCloud2 ToPointCloud2Message(
|
||||||
|
|
||||||
sensor_msgs::PointCloud2 ToPointCloud2Message(
|
sensor_msgs::PointCloud2 ToPointCloud2Message(
|
||||||
const int64 timestamp, const string& frame_id,
|
const int64 timestamp, const string& frame_id,
|
||||||
const ::cartographer::sensor::proto::LaserFan& laser_fan) {
|
const ::cartographer::sensor::proto::RangeData& range_data) {
|
||||||
CHECK(::cartographer::transform::ToEigen(laser_fan.origin()).norm() == 0)
|
CHECK(::cartographer::transform::ToEigen(range_data.origin()).norm() == 0)
|
||||||
<< "Trying to convert a laser_fan that is not at the origin.";
|
<< "Trying to convert a range_data that is not at the origin.";
|
||||||
|
|
||||||
const auto& point_cloud = laser_fan.point_cloud();
|
const auto& point_cloud = range_data.point_cloud();
|
||||||
CHECK_EQ(point_cloud.x_size(), point_cloud.y_size());
|
CHECK_EQ(point_cloud.x_size(), point_cloud.y_size());
|
||||||
CHECK_EQ(point_cloud.x_size(), point_cloud.z_size());
|
CHECK_EQ(point_cloud.x_size(), point_cloud.z_size());
|
||||||
const auto num_points = point_cloud.x_size();
|
const auto num_points = point_cloud.x_size();
|
||||||
|
|
|
@ -50,7 +50,7 @@ sensor_msgs::PointCloud2 ToPointCloud2Message(
|
||||||
|
|
||||||
sensor_msgs::PointCloud2 ToPointCloud2Message(
|
sensor_msgs::PointCloud2 ToPointCloud2Message(
|
||||||
int64 timestamp, const string& frame_id,
|
int64 timestamp, const string& frame_id,
|
||||||
const ::cartographer::sensor::proto::LaserFan& laser_fan);
|
const ::cartographer::sensor::proto::RangeData& range_data);
|
||||||
|
|
||||||
geometry_msgs::Transform ToGeometryMsgTransform(
|
geometry_msgs::Transform ToGeometryMsgTransform(
|
||||||
const ::cartographer::transform::Rigid3d& rigid3d);
|
const ::cartographer::transform::Rigid3d& rigid3d);
|
||||||
|
|
|
@ -36,13 +36,13 @@ void BuildOccupancyGrid(
|
||||||
carto::mapping_2d::MapLimits::ComputeMapLimits(
|
carto::mapping_2d::MapLimits::ComputeMapLimits(
|
||||||
submaps_options.resolution(), trajectory_nodes);
|
submaps_options.resolution(), trajectory_nodes);
|
||||||
carto::mapping_2d::ProbabilityGrid probability_grid(map_limits);
|
carto::mapping_2d::ProbabilityGrid probability_grid(map_limits);
|
||||||
carto::mapping_2d::LaserFanInserter laser_fan_inserter(
|
carto::mapping_2d::RangeDataInserter range_data_inserter(
|
||||||
submaps_options.laser_fan_inserter_options());
|
submaps_options.range_data_inserter_options());
|
||||||
for (const auto& node : trajectory_nodes) {
|
for (const auto& node : trajectory_nodes) {
|
||||||
CHECK(node.constant_data->range_data_3d.returns.empty()); // No 3D yet.
|
CHECK(node.constant_data->range_data_3d.returns.empty()); // No 3D yet.
|
||||||
laser_fan_inserter.Insert(
|
range_data_inserter.Insert(
|
||||||
carto::sensor::TransformLaserFan(node.constant_data->laser_fan_2d,
|
carto::sensor::TransformRangeData(node.constant_data->range_data_2d,
|
||||||
node.pose.cast<float>()),
|
node.pose.cast<float>()),
|
||||||
&probability_grid);
|
&probability_grid);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -33,7 +33,7 @@ options = {
|
||||||
TRAJECTORY_BUILDER_3D.scans_per_accumulation = 180
|
TRAJECTORY_BUILDER_3D.scans_per_accumulation = 180
|
||||||
TRAJECTORY_BUILDER_3D.laser_min_range = 0.5
|
TRAJECTORY_BUILDER_3D.laser_min_range = 0.5
|
||||||
TRAJECTORY_BUILDER_3D.laser_max_range = 20.
|
TRAJECTORY_BUILDER_3D.laser_max_range = 20.
|
||||||
TRAJECTORY_BUILDER_3D.submaps.num_laser_fans = 40.
|
TRAJECTORY_BUILDER_3D.submaps.num_range_data = 40.
|
||||||
|
|
||||||
MAP_BUILDER.use_trajectory_builder_3d = true
|
MAP_BUILDER.use_trajectory_builder_3d = true
|
||||||
MAP_BUILDER.num_background_threads = 7
|
MAP_BUILDER.num_background_threads = 7
|
||||||
|
|
Loading…
Reference in New Issue