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

Add intensities to LocalTrajectoryBuilder3D. () 

Disabled by default.

Signed-off-by: Wolfgang Hess <whess@lyft.com>
master
Wolfgang Hess 2020-10-26 14:47:07 +01:00 committed by GitHub
parent 1f69b83e8a
commit e7f98f3e09
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 52 additions and 17 deletions

51
cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc
View File

@ -82,15 +82,18 @@ std::unique_ptr<transform::Rigid3d> 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::MatchingResult>
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<float>());
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<sensor::RangefinderPoint> accumulated_points;
std::vector<float> 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<transform::Rigid3f>::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<common::Duration> 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<float>(),
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;

2
cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc
View File

@ -132,6 +132,8 @@ class LocalTrajectoryBuilderTest : public ::testing::Test {
intensity_threshold = 100.0,
},
},
use_intensities = false,
}
)text");
return mapping::CreateLocalTrajectoryBuilderOptions3D(

1
cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.cc
View File

@ -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;
}

5
cartographer/mapping/proto/local_trajectory_builder_options_3d.proto
View File

@ -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;
}

10
configuration_files/trajectory_builder_3d.lua
View File

@ -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,
}