diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc index a9bf65f..b2a4f86 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc @@ -82,15 +82,18 @@ std::unique_ptr LocalTrajectoryBuilder3D::ScanMatch( transform::Rigid3d pose_observation_in_submap; ceres::Solver::Summary summary; + const auto* high_resolution_intensity_hybrid_grid = + options_.use_intensities() + ? &matching_submap->high_resolution_intensity_hybrid_grid() + : nullptr; ceres_scan_matcher_->Match( (matching_submap->local_pose().inverse() * pose_prediction).translation(), - initial_ceres_pose, - {{&high_resolution_point_cloud_in_tracking, - &matching_submap->high_resolution_hybrid_grid(), - /*intensity_hybrid_grid=*/nullptr}, - {&low_resolution_point_cloud_in_tracking, - &matching_submap->low_resolution_hybrid_grid(), - /*intensity_hybrid_grid=*/nullptr}}, + initial_ceres_pose, {{&high_resolution_point_cloud_in_tracking, + &matching_submap->high_resolution_hybrid_grid(), + high_resolution_intensity_hybrid_grid}, + {&low_resolution_point_cloud_in_tracking, + &matching_submap->low_resolution_hybrid_grid(), + /*intensity_hybrid_grid=*/nullptr}}, &pose_observation_in_submap, &summary); kCeresScanMatcherCostMetric->Observe(summary.final_cost); const double residual_distance = (pose_observation_in_submap.translation() - @@ -119,7 +122,6 @@ void LocalTrajectoryBuilder3D::AddImuData(const sensor::ImuData& imu_data) { initial_imu_data.push_back(sensor::FromProto(imu)); } initial_imu_data.push_back(imu_data); - extrapolator_ = mapping::PoseExtrapolatorInterface::CreateWithImuData( options_.pose_extrapolator_options(), initial_imu_data, initial_poses); } @@ -128,6 +130,12 @@ std::unique_ptr LocalTrajectoryBuilder3D::AddRangeData( const std::string& sensor_id, const sensor::TimedPointCloudData& unsynchronized_data) { + if (options_.use_intensities()) { + CHECK_EQ(unsynchronized_data.ranges.size(), + unsynchronized_data.intensities.size()) + << "Passed point cloud has inconsistent number of intensities and " + "ranges."; + } auto synchronized_data = range_data_collator_.AddRangeData(sensor_id, unsynchronized_data); if (synchronized_data.ranges.empty()) { @@ -198,7 +206,14 @@ LocalTrajectoryBuilder3D::AddRangeData( hits_poses.push_back(extrapolation_result.current_pose.cast()); CHECK_EQ(hits_poses.size(), hit_times.size()); - sensor::RangeData accumulated_range_data; + const size_t max_possible_number_of_accumulated_points = hit_times.size(); + std::vector accumulated_points; + std::vector accumulated_intensities; + accumulated_points.reserve(max_possible_number_of_accumulated_points); + if (options_.use_intensities()) { + accumulated_intensities.reserve(max_possible_number_of_accumulated_points); + } + sensor::PointCloud misses; std::vector::const_iterator hits_poses_it = hits_poses.begin(); for (const auto& point_cloud_origin_data : @@ -212,13 +227,17 @@ LocalTrajectoryBuilder3D::AddRangeData( const float range = delta.norm(); if (range >= options_.min_range()) { if (range <= options_.max_range()) { - accumulated_range_data.returns.push_back( - sensor::RangefinderPoint{hit_in_local}); + accumulated_points.push_back(sensor::RangefinderPoint{hit_in_local}); + if (options_.use_intensities()) { + accumulated_intensities.push_back(hit.intensity); + } } else { // We insert a ray cropped to 'max_range' as a miss for hits beyond // the maximum range. This way the free space up to the maximum range // will be updated. - accumulated_range_data.misses.push_back(sensor::RangefinderPoint{ + // TODO(wohe): since `misses` are not used anywhere in 3D, consider + // removing `misses` from `range_data` and/or everywhere in 3D. + misses.push_back(sensor::RangefinderPoint{ origin_in_local + options_.max_range() / range * delta}); } } @@ -226,6 +245,8 @@ LocalTrajectoryBuilder3D::AddRangeData( } } CHECK(std::next(hits_poses_it) == hits_poses.end()); + const sensor::PointCloud returns(std::move(accumulated_points), + std::move(accumulated_intensities)); const common::Time current_sensor_time = synchronized_data.time; absl::optional sensor_duration; @@ -238,10 +259,8 @@ LocalTrajectoryBuilder3D::AddRangeData( const auto voxel_filter_start = std::chrono::steady_clock::now(); const sensor::RangeData filtered_range_data = { extrapolation_result.current_pose.translation().cast(), - sensor::VoxelFilter(accumulated_range_data.returns, - options_.voxel_filter_size()), - sensor::VoxelFilter(accumulated_range_data.misses, - options_.voxel_filter_size())}; + sensor::VoxelFilter(returns, options_.voxel_filter_size()), + sensor::VoxelFilter(misses, options_.voxel_filter_size())}; const auto voxel_filter_stop = std::chrono::steady_clock::now(); const auto voxel_filter_duration = voxel_filter_stop - voxel_filter_start; diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc b/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc index e59a1b9..2602649 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc @@ -132,6 +132,8 @@ class LocalTrajectoryBuilderTest : public ::testing::Test { intensity_threshold = 100.0, }, }, + + use_intensities = false, } )text"); return mapping::CreateLocalTrajectoryBuilderOptions3D( diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.cc b/cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.cc index 6dd63ab..860b7d6 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.cc +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.cc @@ -66,6 +66,7 @@ proto::LocalTrajectoryBuilderOptions3D CreateLocalTrajectoryBuilderOptions3D( parameter_dictionary->GetInt("rotational_histogram_size")); *options.mutable_submaps_options() = CreateSubmapsOptions3D( parameter_dictionary->GetDictionary("submaps").get()); + options.set_use_intensities(parameter_dictionary->GetBool("use_intensities")); return options; } diff --git a/cartographer/mapping/proto/local_trajectory_builder_options_3d.proto b/cartographer/mapping/proto/local_trajectory_builder_options_3d.proto index f29debd..2ff762a 100644 --- a/cartographer/mapping/proto/local_trajectory_builder_options_3d.proto +++ b/cartographer/mapping/proto/local_trajectory_builder_options_3d.proto @@ -25,7 +25,7 @@ import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto"; import "cartographer/sensor/proto/sensor.proto"; import "cartographer/transform/proto/timestamped_transform.proto"; -// NEXT ID: 21 +// NEXT ID: 22 message LocalTrajectoryBuilderOptions3D { // Rangefinder points outside these ranges will be dropped. float min_range = 1; @@ -73,4 +73,7 @@ message LocalTrajectoryBuilderOptions3D { repeated sensor.proto.ImuData initial_imu_data = 20; SubmapsOptions3D submaps_options = 8; + + // Whether to use Lidar intensities in Ceres Scan Matcher. + bool use_intensities = 21; } diff --git a/configuration_files/trajectory_builder_3d.lua b/configuration_files/trajectory_builder_3d.lua index 6e3d3b6..489889d 100644 --- a/configuration_files/trajectory_builder_3d.lua +++ b/configuration_files/trajectory_builder_3d.lua @@ -44,6 +44,11 @@ TRAJECTORY_BUILDER_3D = { ceres_scan_matcher = { occupied_space_weight_0 = 1., occupied_space_weight_1 = 6., + intensity_cost_function_options_0 = { + weight = 0.5, + huber_scale = 0.3, + intensity_threshold = INTENSITY_THRESHOLD, + }, translation_weight = 5., rotation_weight = 4e2, only_optimize_yaw = false, @@ -100,4 +105,9 @@ TRAJECTORY_BUILDER_3D = { intensity_threshold = INTENSITY_THRESHOLD, }, }, + + -- When setting use_intensites to true, the intensity_cost_function_options_0 + -- parameter in ceres_scan_matcher has to be set up as well or otherwise + -- CeresScanMatcher will CHECK-fail. + use_intensities = false, }