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

Add intensity data to TimedPointCloudData. (#1742) 

Adds a new field intensities to TimedPointCloudData.
RangeDataCollator now also takes intensities into account.
AddRangeData now takes a point cloud by value instead of
const reference as we would later make a copy of it anyway.

Signed-off-by: Wolfgang Hess <whess@lyft.com>
master
Wolfgang Hess 2020-08-28 10:12:04 +02:00 committed by GitHub
parent da779339fa
commit a20db758cd
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 90 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
cartographer/mapping/internal/range_data_collator.cc
View File

@ -25,10 +25,14 @@
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping {
constexpr float RangeDataCollator::kDefaultIntensityValue;
sensor::TimedPointCloudOriginData RangeDataCollator::AddRangeData( sensor::TimedPointCloudOriginData RangeDataCollator::AddRangeData(
const std::string& sensor_id, const std::string& sensor_id,
const sensor::TimedPointCloudData& timed_point_cloud_data) { sensor::TimedPointCloudData timed_point_cloud_data) {
CHECK_NE(expected_sensor_ids_.count(sensor_id), 0); CHECK_NE(expected_sensor_ids_.count(sensor_id), 0);
timed_point_cloud_data.intensities.resize(
timed_point_cloud_data.ranges.size(), kDefaultIntensityValue);
// TODO(gaschler): These two cases can probably be one. // TODO(gaschler): These two cases can probably be one.
if (id_to_pending_data_.count(sensor_id) != 0) { if (id_to_pending_data_.count(sensor_id) != 0) {
current_start_ = current_end_; current_start_ = current_end_;
@ -36,10 +40,10 @@ sensor::TimedPointCloudOriginData RangeDataCollator::AddRangeData(
// the two (do not send out current). // the two (do not send out current).
current_end_ = id_to_pending_data_.at(sensor_id).time; current_end_ = id_to_pending_data_.at(sensor_id).time;
auto result = CropAndMerge(); auto result = CropAndMerge();
id_to_pending_data_.emplace(sensor_id, timed_point_cloud_data); id_to_pending_data_.emplace(sensor_id, std::move(timed_point_cloud_data));
return result; return result;
} }
id_to_pending_data_.emplace(sensor_id, timed_point_cloud_data); id_to_pending_data_.emplace(sensor_id, std::move(timed_point_cloud_data));
if (expected_sensor_ids_.size() != id_to_pending_data_.size()) { if (expected_sensor_ids_.size() != id_to_pending_data_.size()) {
return {}; return {};
} }
@ -59,7 +63,8 @@ sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge() {
for (auto it = id_to_pending_data_.begin(); for (auto it = id_to_pending_data_.begin();
it != id_to_pending_data_.end();) { it != id_to_pending_data_.end();) {
sensor::TimedPointCloudData& data = it->second; sensor::TimedPointCloudData& data = it->second;
sensor::TimedPointCloud& ranges = it->second.ranges; const sensor::TimedPointCloud& ranges = it->second.ranges;
const std::vector<float>& intensities = it->second.intensities;
auto overlap_begin = ranges.begin(); auto overlap_begin = ranges.begin();
while (overlap_begin < ranges.end() && while (overlap_begin < ranges.end() &&
@ -85,10 +90,14 @@ sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge() {
result.origins.push_back(data.origin); result.origins.push_back(data.origin);
const float time_correction = const float time_correction =
static_cast<float>(common::ToSeconds(data.time - current_end_)); static_cast<float>(common::ToSeconds(data.time - current_end_));
auto intensities_overlap_it =
intensities.begin() + (overlap_begin - ranges.begin());
result.ranges.reserve(result.ranges.size() +
std::distance(overlap_begin, overlap_end));
for (auto overlap_it = overlap_begin; overlap_it != overlap_end; for (auto overlap_it = overlap_begin; overlap_it != overlap_end;
++overlap_it) { ++overlap_it, ++intensities_overlap_it) {
sensor::TimedPointCloudOriginData::RangeMeasurement point{*overlap_it, sensor::TimedPointCloudOriginData::RangeMeasurement point{
origin_index}; *overlap_it, *intensities_overlap_it, origin_index};
// current_end_ + point_time[3]_after == in_timestamp + // current_end_ + point_time[3]_after == in_timestamp +
// point_time[3]_before // point_time[3]_before
point.point_time.time += time_correction; point.point_time.time += time_correction;
@ -102,9 +111,12 @@ sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge() {
} else if (overlap_end == ranges.begin()) { } else if (overlap_end == ranges.begin()) {
++it; ++it;
} else { } else {
const auto intensities_overlap_end =
intensities.begin() + (overlap_end - ranges.begin());
data = sensor::TimedPointCloudData{ data = sensor::TimedPointCloudData{
data.time, data.origin, data.time, data.origin,
sensor::TimedPointCloud(overlap_end, ranges.end())}; sensor::TimedPointCloud(overlap_end, ranges.end()),
std::vector<float>(intensities_overlap_end, intensities.end())};
++it; ++it;
} }
} }

6
cartographer/mapping/internal/range_data_collator.h
View File

@ -37,9 +37,11 @@ class RangeDataCollator {
: expected_sensor_ids_(expected_range_sensor_ids.begin(), : expected_sensor_ids_(expected_range_sensor_ids.begin(),
expected_range_sensor_ids.end()) {} expected_range_sensor_ids.end()) {}
// If timed_point_cloud_data has incomplete intensity data, we will fill the
// missing intensities with kDefaultIntensityValue.
sensor::TimedPointCloudOriginData AddRangeData( sensor::TimedPointCloudOriginData AddRangeData(
const std::string& sensor_id, const std::string& sensor_id,
const sensor::TimedPointCloudData& timed_point_cloud_data); sensor::TimedPointCloudData timed_point_cloud_data);
private: private:
sensor::TimedPointCloudOriginData CropAndMerge(); sensor::TimedPointCloudOriginData CropAndMerge();
@ -49,6 +51,8 @@ class RangeDataCollator {
std::map<std::string, sensor::TimedPointCloudData> id_to_pending_data_; std::map<std::string, sensor::TimedPointCloudData> id_to_pending_data_;
common::Time current_start_ = common::Time::min(); common::Time current_start_ = common::Time::min();
common::Time current_end_ = common::Time::min(); common::Time current_end_ = common::Time::min();
constexpr static float kDefaultIntensityValue = 0.f;
}; };
} // namespace mapping } // namespace mapping

71
cartographer/mapping/internal/range_data_collator_test.cc
View File

@ -26,16 +26,21 @@ namespace {
const int kNumSamples = 10; const int kNumSamples = 10;
sensor::TimedPointCloudData CreateFakeRangeData(int from, int to) { sensor::TimedPointCloudData CreateFakeRangeData(int from, int to,
bool fake_intensities) {
double duration = common::ToSeconds(common::FromUniversal(to) - double duration = common::ToSeconds(common::FromUniversal(to) -
common::FromUniversal(from)); common::FromUniversal(from));
sensor::TimedPointCloudData result{ sensor::TimedPointCloudData result{
common::FromUniversal(to), Eigen::Vector3f(0., 1., 2.), {}}; common::FromUniversal(to), Eigen::Vector3f(0., 1., 2.), {}, {}};
result.ranges.reserve(kNumSamples); result.ranges.reserve(kNumSamples);
for (int i = 0; i < kNumSamples; ++i) { for (int i = 0; i < kNumSamples; ++i) {
double fraction = static_cast<double>(i) / (kNumSamples - 1); double fraction = static_cast<double>(i) / (kNumSamples - 1);
float relative_time = (1.f - fraction) * -duration; float relative_time = (1. - fraction) * -duration;
result.ranges.push_back({Eigen::Vector3f{1., 2., 3.}, relative_time}); result.ranges.push_back(
{Eigen::Vector3f{1., 2., static_cast<float>(fraction)}, relative_time});
if (fake_intensities) {
result.intensities.push_back(result.ranges.back().position.z());
}
} }
return result; return result;
} }
@ -49,17 +54,23 @@ bool ArePointTimestampsSorted(const sensor::TimedPointCloudOriginData& data) {
return std::is_sorted(timestamps.begin(), timestamps.end()); return std::is_sorted(timestamps.begin(), timestamps.end());
} }
void IntensitiesAreConsistent(const sensor::TimedPointCloudOriginData& data) {
for (const auto& range : data.ranges) {
EXPECT_NEAR(range.point_time.position.z(), range.intensity, 1e-6);
}
}
TEST(RangeDataCollatorTest, SingleSensor) { TEST(RangeDataCollatorTest, SingleSensor) {
const std::string sensor_id = "single_sensor"; const std::string sensor_id = "single_sensor";
RangeDataCollator collator({sensor_id}); RangeDataCollator collator({sensor_id});
auto output_0 = auto output_0 =
collator.AddRangeData(sensor_id, CreateFakeRangeData(200, 300)); collator.AddRangeData(sensor_id, CreateFakeRangeData(200, 300, false));
EXPECT_EQ(common::ToUniversal(output_0.time), 300); EXPECT_EQ(common::ToUniversal(output_0.time), 300);
EXPECT_EQ(output_0.origins.size(), 1); EXPECT_EQ(output_0.origins.size(), 1);
EXPECT_EQ(output_0.ranges.size(), kNumSamples); EXPECT_EQ(output_0.ranges.size(), kNumSamples);
EXPECT_TRUE(ArePointTimestampsSorted(output_0)); EXPECT_TRUE(ArePointTimestampsSorted(output_0));
auto output_1 = auto output_1 =
collator.AddRangeData(sensor_id, CreateFakeRangeData(300, 500)); collator.AddRangeData(sensor_id, CreateFakeRangeData(300, 500, false));
EXPECT_EQ(common::ToUniversal(output_1.time), 500); EXPECT_EQ(common::ToUniversal(output_1.time), 500);
EXPECT_EQ(output_1.origins.size(), 1); EXPECT_EQ(output_1.origins.size(), 1);
ASSERT_EQ(output_1.ranges.size(), kNumSamples); ASSERT_EQ(output_1.ranges.size(), kNumSamples);
@ -69,7 +80,7 @@ TEST(RangeDataCollatorTest, SingleSensor) {
common::FromSeconds(output_1.ranges[0].point_time.time)), common::FromSeconds(output_1.ranges[0].point_time.time)),
300, 2); 300, 2);
auto output_2 = auto output_2 =
collator.AddRangeData(sensor_id, CreateFakeRangeData(-1000, 510)); collator.AddRangeData(sensor_id, CreateFakeRangeData(-1000, 510, false));
EXPECT_EQ(common::ToUniversal(output_2.time), 510); EXPECT_EQ(common::ToUniversal(output_2.time), 510);
EXPECT_EQ(output_2.origins.size(), 1); EXPECT_EQ(output_2.origins.size(), 1);
EXPECT_EQ(output_2.ranges.size(), 1); EXPECT_EQ(output_2.ranges.size(), 1);
@ -80,13 +91,14 @@ TEST(RangeDataCollatorTest, SingleSensor) {
TEST(RangeDataCollatorTest, SingleSensorEmptyData) { TEST(RangeDataCollatorTest, SingleSensorEmptyData) {
const std::string sensor_id = "single_sensor"; const std::string sensor_id = "single_sensor";
RangeDataCollator collator({sensor_id}); RangeDataCollator collator({sensor_id});
sensor::TimedPointCloudData empty_data{common::FromUniversal(300)}; sensor::TimedPointCloudData empty_data{
common::FromUniversal(300), {}, {}, {}};
auto output_0 = collator.AddRangeData(sensor_id, empty_data); auto output_0 = collator.AddRangeData(sensor_id, empty_data);
EXPECT_EQ(output_0.time, empty_data.time); EXPECT_EQ(output_0.time, empty_data.time);
EXPECT_EQ(output_0.ranges.size(), empty_data.ranges.size()); EXPECT_EQ(output_0.ranges.size(), empty_data.ranges.size());
EXPECT_TRUE(ArePointTimestampsSorted(output_0)); EXPECT_TRUE(ArePointTimestampsSorted(output_0));
auto output_1 = auto output_1 =
collator.AddRangeData(sensor_id, CreateFakeRangeData(300, 500)); collator.AddRangeData(sensor_id, CreateFakeRangeData(300, 500, false));
EXPECT_EQ(common::ToUniversal(output_1.time), 500); EXPECT_EQ(common::ToUniversal(output_1.time), 500);
EXPECT_EQ(output_1.origins.size(), 1); EXPECT_EQ(output_1.origins.size(), 1);
ASSERT_EQ(output_1.ranges.size(), kNumSamples); ASSERT_EQ(output_1.ranges.size(), kNumSamples);
@ -96,7 +108,7 @@ TEST(RangeDataCollatorTest, SingleSensorEmptyData) {
common::FromSeconds(output_1.ranges[0].point_time.time)), common::FromSeconds(output_1.ranges[0].point_time.time)),
300, 2); 300, 2);
auto output_2 = auto output_2 =
collator.AddRangeData(sensor_id, CreateFakeRangeData(-1000, 510)); collator.AddRangeData(sensor_id, CreateFakeRangeData(-1000, 510, false));
EXPECT_EQ(common::ToUniversal(output_2.time), 510); EXPECT_EQ(common::ToUniversal(output_2.time), 510);
EXPECT_EQ(output_2.origins.size(), 1); EXPECT_EQ(output_2.origins.size(), 1);
EXPECT_EQ(output_2.ranges.size(), 1); EXPECT_EQ(output_2.ranges.size(), 1);
@ -109,10 +121,10 @@ TEST(RangeDataCollatorTest, TwoSensors) {
const std::string sensor_1 = "sensor_1"; const std::string sensor_1 = "sensor_1";
RangeDataCollator collator({sensor_0, sensor_1}); RangeDataCollator collator({sensor_0, sensor_1});
auto output_0 = auto output_0 =
collator.AddRangeData(sensor_0, CreateFakeRangeData(200, 300)); collator.AddRangeData(sensor_0, CreateFakeRangeData(200, 300, false));
EXPECT_EQ(output_0.ranges.size(), 0); EXPECT_EQ(output_0.ranges.size(), 0);
auto output_1 = auto output_1 =
collator.AddRangeData(sensor_1, CreateFakeRangeData(-1000, 310)); collator.AddRangeData(sensor_1, CreateFakeRangeData(-1000, 310, false));
EXPECT_EQ(output_1.origins.size(), 2); EXPECT_EQ(output_1.origins.size(), 2);
EXPECT_EQ(common::ToUniversal(output_1.time), 300); EXPECT_EQ(common::ToUniversal(output_1.time), 300);
ASSERT_EQ(output_1.ranges.size(), 2 * kNumSamples - 1); ASSERT_EQ(output_1.ranges.size(), 2 * kNumSamples - 1);
@ -123,7 +135,7 @@ TEST(RangeDataCollatorTest, TwoSensors) {
EXPECT_EQ(output_1.ranges.back().point_time.time, 0.f); EXPECT_EQ(output_1.ranges.back().point_time.time, 0.f);
EXPECT_TRUE(ArePointTimestampsSorted(output_1)); EXPECT_TRUE(ArePointTimestampsSorted(output_1));
auto output_2 = auto output_2 =
collator.AddRangeData(sensor_0, CreateFakeRangeData(300, 500)); collator.AddRangeData(sensor_0, CreateFakeRangeData(300, 500, false));
EXPECT_EQ(output_2.origins.size(), 2); EXPECT_EQ(output_2.origins.size(), 2);
EXPECT_EQ(common::ToUniversal(output_2.time), 310); EXPECT_EQ(common::ToUniversal(output_2.time), 310);
ASSERT_EQ(output_2.ranges.size(), 2); ASSERT_EQ(output_2.ranges.size(), 2);
@ -135,7 +147,7 @@ TEST(RangeDataCollatorTest, TwoSensors) {
EXPECT_TRUE(ArePointTimestampsSorted(output_2)); EXPECT_TRUE(ArePointTimestampsSorted(output_2));
// Sending the same sensor will flush everything before. // Sending the same sensor will flush everything before.
auto output_3 = auto output_3 =
collator.AddRangeData(sensor_0, CreateFakeRangeData(600, 700)); collator.AddRangeData(sensor_0, CreateFakeRangeData(600, 700, false));
EXPECT_EQ(common::ToUniversal(output_3.time), 500); EXPECT_EQ(common::ToUniversal(output_3.time), 500);
EXPECT_EQ( EXPECT_EQ(
output_1.ranges.size() + output_2.ranges.size() + output_3.ranges.size(), output_1.ranges.size() + output_2.ranges.size() + output_3.ranges.size(),
@ -150,21 +162,44 @@ TEST(RangeDataCollatorTest, ThreeSensors) {
const std::string sensor_2 = "sensor_2"; const std::string sensor_2 = "sensor_2";
RangeDataCollator collator({sensor_0, sensor_1, sensor_2}); RangeDataCollator collator({sensor_0, sensor_1, sensor_2});
auto output_0 = auto output_0 =
collator.AddRangeData(sensor_0, CreateFakeRangeData(100, 200)); collator.AddRangeData(sensor_0, CreateFakeRangeData(100, 200, false));
EXPECT_EQ(output_0.ranges.size(), 0); EXPECT_EQ(output_0.ranges.size(), 0);
auto output_1 = auto output_1 =
collator.AddRangeData(sensor_1, CreateFakeRangeData(199, 250)); collator.AddRangeData(sensor_1, CreateFakeRangeData(199, 250, false));
EXPECT_EQ(output_1.ranges.size(), 0); EXPECT_EQ(output_1.ranges.size(), 0);
auto output_2 = auto output_2 =
collator.AddRangeData(sensor_2, CreateFakeRangeData(210, 300)); collator.AddRangeData(sensor_2, CreateFakeRangeData(210, 300, false));
EXPECT_EQ(output_2.ranges.size(), kNumSamples + 1); EXPECT_EQ(output_2.ranges.size(), kNumSamples + 1);
EXPECT_TRUE(ArePointTimestampsSorted(output_2)); EXPECT_TRUE(ArePointTimestampsSorted(output_2));
auto output_3 = auto output_3 =
collator.AddRangeData(sensor_2, CreateFakeRangeData(400, 500)); collator.AddRangeData(sensor_2, CreateFakeRangeData(400, 500, false));
EXPECT_EQ(output_2.ranges.size() + output_3.ranges.size(), 3 * kNumSamples); EXPECT_EQ(output_2.ranges.size() + output_3.ranges.size(), 3 * kNumSamples);
EXPECT_TRUE(ArePointTimestampsSorted(output_3)); EXPECT_TRUE(ArePointTimestampsSorted(output_3));
} }
TEST(RangeDataCollatorTest, ThreeSensorsWithIntensities) {
const std::string sensor_0 = "sensor_0";
const std::string sensor_1 = "sensor_1";
const std::string sensor_2 = "sensor_2";
RangeDataCollator collator({sensor_0, sensor_1, sensor_2});
auto output_0 =
collator.AddRangeData(sensor_0, CreateFakeRangeData(100, 200, true));
EXPECT_EQ(output_0.ranges.size(), 0);
auto output_1 =
collator.AddRangeData(sensor_1, CreateFakeRangeData(199, 250, true));
EXPECT_EQ(output_1.ranges.size(), 0);
auto output_2 =
collator.AddRangeData(sensor_2, CreateFakeRangeData(210, 300, true));
EXPECT_EQ(output_2.ranges.size(), kNumSamples + 1);
EXPECT_TRUE(ArePointTimestampsSorted(output_2));
IntensitiesAreConsistent(output_2);
auto output_3 =
collator.AddRangeData(sensor_2, CreateFakeRangeData(400, 500, true));
EXPECT_EQ(output_2.ranges.size() + output_3.ranges.size(), 3 * kNumSamples);
EXPECT_TRUE(ArePointTimestampsSorted(output_3));
IntensitiesAreConsistent(output_3);
}
} // namespace } // namespace
} // namespace mapping } // namespace mapping
} // namespace cartographer } // namespace cartographer

1
cartographer/sensor/proto/sensor.proto
View File

@ -41,6 +41,7 @@ message TimedPointCloudData {
transform.proto.Vector3f origin = 2; transform.proto.Vector3f origin = 2;
repeated transform.proto.Vector4f point_data_legacy = 3; repeated transform.proto.Vector4f point_data_legacy = 3;
repeated TimedRangefinderPoint point_data = 4; repeated TimedRangefinderPoint point_data = 4;
repeated float intensities = 5;
} }
// Proto representation of ::cartographer::sensor::RangeData. // Proto representation of ::cartographer::sensor::RangeData.

9
cartographer/sensor/timed_point_cloud_data.cc
View File

@ -31,10 +31,15 @@ proto::TimedPointCloudData ToProto(
for (const TimedRangefinderPoint& range : timed_point_cloud_data.ranges) { for (const TimedRangefinderPoint& range : timed_point_cloud_data.ranges) {
*proto.add_point_data() = ToProto(range); *proto.add_point_data() = ToProto(range);
} }
for (const float intensity : timed_point_cloud_data.intensities) {
proto.add_intensities(intensity);
}
return proto; return proto;
} }
TimedPointCloudData FromProto(const proto::TimedPointCloudData& proto) { TimedPointCloudData FromProto(const proto::TimedPointCloudData& proto) {
CHECK(proto.intensities().size() == 0 ||
proto.intensities().size() == proto.point_data().size());
TimedPointCloud timed_point_cloud; TimedPointCloud timed_point_cloud;
if (proto.point_data().size() > 0) { if (proto.point_data().size() > 0) {
timed_point_cloud.reserve(proto.point_data().size()); timed_point_cloud.reserve(proto.point_data().size());
@ -50,7 +55,9 @@ TimedPointCloudData FromProto(const proto::TimedPointCloudData& proto) {
} }
return TimedPointCloudData{common::FromUniversal(proto.timestamp()), return TimedPointCloudData{common::FromUniversal(proto.timestamp()),
transform::ToEigen(proto.origin()), transform::ToEigen(proto.origin()),
timed_point_cloud}; timed_point_cloud,
std::vector<float>(proto.intensities().begin(),
proto.intensities().end())};
} }
} // namespace sensor } // namespace sensor

3
cartographer/sensor/timed_point_cloud_data.h
View File

@ -28,11 +28,14 @@ struct TimedPointCloudData {
common::Time time; common::Time time;
Eigen::Vector3f origin; Eigen::Vector3f origin;
TimedPointCloud ranges; TimedPointCloud ranges;
// 'intensities' has to be same size as 'ranges', or empty.
std::vector<float> intensities;
}; };
struct TimedPointCloudOriginData { struct TimedPointCloudOriginData {
struct RangeMeasurement { struct RangeMeasurement {
TimedRangefinderPoint point_time; TimedRangefinderPoint point_time;
float intensity;
size_t origin_index; size_t origin_index;
}; };
common::Time time; common::Time time;