Add intensities to LocalTrajectoryBuilder3D. (#1766)
Disabled by default. Signed-off-by: Wolfgang Hess <whess@lyft.com>master
parent
1f69b83e8a
commit
e7f98f3e09
|
@ -82,15 +82,18 @@ std::unique_ptr<transform::Rigid3d> LocalTrajectoryBuilder3D::ScanMatch(
|
||||||
|
|
||||||
transform::Rigid3d pose_observation_in_submap;
|
transform::Rigid3d pose_observation_in_submap;
|
||||||
ceres::Solver::Summary summary;
|
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(
|
ceres_scan_matcher_->Match(
|
||||||
(matching_submap->local_pose().inverse() * pose_prediction).translation(),
|
(matching_submap->local_pose().inverse() * pose_prediction).translation(),
|
||||||
initial_ceres_pose,
|
initial_ceres_pose, {{&high_resolution_point_cloud_in_tracking,
|
||||||
{{&high_resolution_point_cloud_in_tracking,
|
&matching_submap->high_resolution_hybrid_grid(),
|
||||||
&matching_submap->high_resolution_hybrid_grid(),
|
high_resolution_intensity_hybrid_grid},
|
||||||
/*intensity_hybrid_grid=*/nullptr},
|
{&low_resolution_point_cloud_in_tracking,
|
||||||
{&low_resolution_point_cloud_in_tracking,
|
&matching_submap->low_resolution_hybrid_grid(),
|
||||||
&matching_submap->low_resolution_hybrid_grid(),
|
/*intensity_hybrid_grid=*/nullptr}},
|
||||||
/*intensity_hybrid_grid=*/nullptr}},
|
|
||||||
&pose_observation_in_submap, &summary);
|
&pose_observation_in_submap, &summary);
|
||||||
kCeresScanMatcherCostMetric->Observe(summary.final_cost);
|
kCeresScanMatcherCostMetric->Observe(summary.final_cost);
|
||||||
const double residual_distance = (pose_observation_in_submap.translation() -
|
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(sensor::FromProto(imu));
|
||||||
}
|
}
|
||||||
initial_imu_data.push_back(imu_data);
|
initial_imu_data.push_back(imu_data);
|
||||||
|
|
||||||
extrapolator_ = mapping::PoseExtrapolatorInterface::CreateWithImuData(
|
extrapolator_ = mapping::PoseExtrapolatorInterface::CreateWithImuData(
|
||||||
options_.pose_extrapolator_options(), initial_imu_data, initial_poses);
|
options_.pose_extrapolator_options(), initial_imu_data, initial_poses);
|
||||||
}
|
}
|
||||||
|
@ -128,6 +130,12 @@ std::unique_ptr<LocalTrajectoryBuilder3D::MatchingResult>
|
||||||
LocalTrajectoryBuilder3D::AddRangeData(
|
LocalTrajectoryBuilder3D::AddRangeData(
|
||||||
const std::string& sensor_id,
|
const std::string& sensor_id,
|
||||||
const sensor::TimedPointCloudData& unsynchronized_data) {
|
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 =
|
auto synchronized_data =
|
||||||
range_data_collator_.AddRangeData(sensor_id, unsynchronized_data);
|
range_data_collator_.AddRangeData(sensor_id, unsynchronized_data);
|
||||||
if (synchronized_data.ranges.empty()) {
|
if (synchronized_data.ranges.empty()) {
|
||||||
|
@ -198,7 +206,14 @@ LocalTrajectoryBuilder3D::AddRangeData(
|
||||||
hits_poses.push_back(extrapolation_result.current_pose.cast<float>());
|
hits_poses.push_back(extrapolation_result.current_pose.cast<float>());
|
||||||
CHECK_EQ(hits_poses.size(), hit_times.size());
|
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 =
|
std::vector<transform::Rigid3f>::const_iterator hits_poses_it =
|
||||||
hits_poses.begin();
|
hits_poses.begin();
|
||||||
for (const auto& point_cloud_origin_data :
|
for (const auto& point_cloud_origin_data :
|
||||||
|
@ -212,13 +227,17 @@ LocalTrajectoryBuilder3D::AddRangeData(
|
||||||
const float range = delta.norm();
|
const float range = delta.norm();
|
||||||
if (range >= options_.min_range()) {
|
if (range >= options_.min_range()) {
|
||||||
if (range <= options_.max_range()) {
|
if (range <= options_.max_range()) {
|
||||||
accumulated_range_data.returns.push_back(
|
accumulated_points.push_back(sensor::RangefinderPoint{hit_in_local});
|
||||||
sensor::RangefinderPoint{hit_in_local});
|
if (options_.use_intensities()) {
|
||||||
|
accumulated_intensities.push_back(hit.intensity);
|
||||||
|
}
|
||||||
} else {
|
} else {
|
||||||
// We insert a ray cropped to 'max_range' as a miss for hits beyond
|
// 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
|
// the maximum range. This way the free space up to the maximum range
|
||||||
// will be updated.
|
// 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});
|
origin_in_local + options_.max_range() / range * delta});
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -226,6 +245,8 @@ LocalTrajectoryBuilder3D::AddRangeData(
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
CHECK(std::next(hits_poses_it) == hits_poses.end());
|
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;
|
const common::Time current_sensor_time = synchronized_data.time;
|
||||||
absl::optional<common::Duration> sensor_duration;
|
absl::optional<common::Duration> sensor_duration;
|
||||||
|
@ -238,10 +259,8 @@ LocalTrajectoryBuilder3D::AddRangeData(
|
||||||
const auto voxel_filter_start = std::chrono::steady_clock::now();
|
const auto voxel_filter_start = std::chrono::steady_clock::now();
|
||||||
const sensor::RangeData filtered_range_data = {
|
const sensor::RangeData filtered_range_data = {
|
||||||
extrapolation_result.current_pose.translation().cast<float>(),
|
extrapolation_result.current_pose.translation().cast<float>(),
|
||||||
sensor::VoxelFilter(accumulated_range_data.returns,
|
sensor::VoxelFilter(returns, options_.voxel_filter_size()),
|
||||||
options_.voxel_filter_size()),
|
sensor::VoxelFilter(misses, options_.voxel_filter_size())};
|
||||||
sensor::VoxelFilter(accumulated_range_data.misses,
|
|
||||||
options_.voxel_filter_size())};
|
|
||||||
const auto voxel_filter_stop = std::chrono::steady_clock::now();
|
const auto voxel_filter_stop = std::chrono::steady_clock::now();
|
||||||
const auto voxel_filter_duration = voxel_filter_stop - voxel_filter_start;
|
const auto voxel_filter_duration = voxel_filter_stop - voxel_filter_start;
|
||||||
|
|
||||||
|
|
|
@ -132,6 +132,8 @@ class LocalTrajectoryBuilderTest : public ::testing::Test {
|
||||||
intensity_threshold = 100.0,
|
intensity_threshold = 100.0,
|
||||||
},
|
},
|
||||||
},
|
},
|
||||||
|
|
||||||
|
use_intensities = false,
|
||||||
}
|
}
|
||||||
)text");
|
)text");
|
||||||
return mapping::CreateLocalTrajectoryBuilderOptions3D(
|
return mapping::CreateLocalTrajectoryBuilderOptions3D(
|
||||||
|
|
|
@ -66,6 +66,7 @@ proto::LocalTrajectoryBuilderOptions3D CreateLocalTrajectoryBuilderOptions3D(
|
||||||
parameter_dictionary->GetInt("rotational_histogram_size"));
|
parameter_dictionary->GetInt("rotational_histogram_size"));
|
||||||
*options.mutable_submaps_options() = CreateSubmapsOptions3D(
|
*options.mutable_submaps_options() = CreateSubmapsOptions3D(
|
||||||
parameter_dictionary->GetDictionary("submaps").get());
|
parameter_dictionary->GetDictionary("submaps").get());
|
||||||
|
options.set_use_intensities(parameter_dictionary->GetBool("use_intensities"));
|
||||||
return options;
|
return options;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -25,7 +25,7 @@ import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto";
|
||||||
import "cartographer/sensor/proto/sensor.proto";
|
import "cartographer/sensor/proto/sensor.proto";
|
||||||
import "cartographer/transform/proto/timestamped_transform.proto";
|
import "cartographer/transform/proto/timestamped_transform.proto";
|
||||||
|
|
||||||
// NEXT ID: 21
|
// NEXT ID: 22
|
||||||
message LocalTrajectoryBuilderOptions3D {
|
message LocalTrajectoryBuilderOptions3D {
|
||||||
// Rangefinder points outside these ranges will be dropped.
|
// Rangefinder points outside these ranges will be dropped.
|
||||||
float min_range = 1;
|
float min_range = 1;
|
||||||
|
@ -73,4 +73,7 @@ message LocalTrajectoryBuilderOptions3D {
|
||||||
repeated sensor.proto.ImuData initial_imu_data = 20;
|
repeated sensor.proto.ImuData initial_imu_data = 20;
|
||||||
|
|
||||||
SubmapsOptions3D submaps_options = 8;
|
SubmapsOptions3D submaps_options = 8;
|
||||||
|
|
||||||
|
// Whether to use Lidar intensities in Ceres Scan Matcher.
|
||||||
|
bool use_intensities = 21;
|
||||||
}
|
}
|
||||||
|
|
|
@ -44,6 +44,11 @@ TRAJECTORY_BUILDER_3D = {
|
||||||
ceres_scan_matcher = {
|
ceres_scan_matcher = {
|
||||||
occupied_space_weight_0 = 1.,
|
occupied_space_weight_0 = 1.,
|
||||||
occupied_space_weight_1 = 6.,
|
occupied_space_weight_1 = 6.,
|
||||||
|
intensity_cost_function_options_0 = {
|
||||||
|
weight = 0.5,
|
||||||
|
huber_scale = 0.3,
|
||||||
|
intensity_threshold = INTENSITY_THRESHOLD,
|
||||||
|
},
|
||||||
translation_weight = 5.,
|
translation_weight = 5.,
|
||||||
rotation_weight = 4e2,
|
rotation_weight = 4e2,
|
||||||
only_optimize_yaw = false,
|
only_optimize_yaw = false,
|
||||||
|
@ -100,4 +105,9 @@ TRAJECTORY_BUILDER_3D = {
|
||||||
intensity_threshold = INTENSITY_THRESHOLD,
|
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,
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue