Skip to content

Commit

Permalink
Add intensity data to TimedPointCloudData. (cartographer-project#1742)
Browse files Browse the repository at this point in the history 
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 <[email protected]>
  • Loading branch information
@wohe
wohe authored Aug 28, 2020
1 parent da77933 commit a20db75
Show file tree
Hide file tree
Showing 6 changed files with 90 additions and 28 deletions.
28 changes: 20 additions & 8 deletions cartographer/mapping/internal/range_data_collator.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,21 +25,25 @@
namespace cartographer {
namespace mapping {

constexpr float RangeDataCollator::kDefaultIntensityValue;

sensor::TimedPointCloudOriginData RangeDataCollator::AddRangeData(
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);
timed_point_cloud_data.intensities.resize(
timed_point_cloud_data.ranges.size(), kDefaultIntensityValue);
// TODO(gaschler): These two cases can probably be one.
if (id_to_pending_data_.count(sensor_id) != 0) {
current_start_ = current_end_;
// When we have two messages of the same sensor, move forward the older of
// the two (do not send out current).
current_end_ = id_to_pending_data_.at(sensor_id).time;
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;
}
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()) {
return {};
}
Expand All @@ -59,7 +63,8 @@ sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge() {
for (auto it = id_to_pending_data_.begin();
it != id_to_pending_data_.end();) {
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();
while (overlap_begin < ranges.end() &&
Expand All @@ -85,10 +90,14 @@ sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge() {
result.origins.push_back(data.origin);
const float time_correction =
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;
++overlap_it) {
sensor::TimedPointCloudOriginData::RangeMeasurement point{*overlap_it,
origin_index};
++overlap_it, ++intensities_overlap_it) {
sensor::TimedPointCloudOriginData::RangeMeasurement point{
*overlap_it, *intensities_overlap_it, origin_index};
// current_end_ + point_time[3]_after == in_timestamp +
// point_time[3]_before
point.point_time.time += time_correction;
Expand All @@ -102,9 +111,12 @@ sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge() {
} else if (overlap_end == ranges.begin()) {
++it;
} else {
const auto intensities_overlap_end =
intensities.begin() + (overlap_end - ranges.begin());
data = sensor::TimedPointCloudData{
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;
}
}
Expand Down
6 changes: 5 additions & 1 deletion cartographer/mapping/internal/range_data_collator.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,9 +37,11 @@ class RangeDataCollator {
: expected_sensor_ids_(expected_range_sensor_ids.begin(),
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(
const std::string& sensor_id,
const sensor::TimedPointCloudData& timed_point_cloud_data);
sensor::TimedPointCloudData timed_point_cloud_data);

private:
sensor::TimedPointCloudOriginData CropAndMerge();
Expand All @@ -49,6 +51,8 @@ class RangeDataCollator {
std::map<std::string, sensor::TimedPointCloudData> id_to_pending_data_;
common::Time current_start_ = common::Time::min();
common::Time current_end_ = common::Time::min();

constexpr static float kDefaultIntensityValue = 0.f;
};

} // namespace mapping
Expand Down
71 changes: 53 additions & 18 deletions cartographer/mapping/internal/range_data_collator_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,16 +26,21 @@ namespace {

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) -
common::FromUniversal(from));
sensor::TimedPointCloudData result{
common::FromUniversal(to), Eigen::Vector3f(0., 1., 2.), {}};
common::FromUniversal(to), Eigen::Vector3f(0., 1., 2.), {}, {}};
result.ranges.reserve(kNumSamples);
for (int i = 0; i < kNumSamples; ++i) {
double fraction = static_cast<double>(i) / (kNumSamples - 1);
float relative_time = (1.f - fraction) * -duration;
result.ranges.push_back({Eigen::Vector3f{1., 2., 3.}, relative_time});
float relative_time = (1. - fraction) * -duration;
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;
}
Expand All @@ -49,17 +54,23 @@ bool ArePointTimestampsSorted(const sensor::TimedPointCloudOriginData& data) {
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) {
const std::string sensor_id = "single_sensor";
RangeDataCollator collator({sensor_id});
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(output_0.origins.size(), 1);
EXPECT_EQ(output_0.ranges.size(), kNumSamples);
EXPECT_TRUE(ArePointTimestampsSorted(output_0));
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(output_1.origins.size(), 1);
ASSERT_EQ(output_1.ranges.size(), kNumSamples);
Expand All @@ -69,7 +80,7 @@ TEST(RangeDataCollatorTest, SingleSensor) {
common::FromSeconds(output_1.ranges[0].point_time.time)),
300, 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(output_2.origins.size(), 1);
EXPECT_EQ(output_2.ranges.size(), 1);
Expand All @@ -80,13 +91,14 @@ TEST(RangeDataCollatorTest, SingleSensor) {
TEST(RangeDataCollatorTest, SingleSensorEmptyData) {
const std::string sensor_id = "single_sensor";
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);
EXPECT_EQ(output_0.time, empty_data.time);
EXPECT_EQ(output_0.ranges.size(), empty_data.ranges.size());
EXPECT_TRUE(ArePointTimestampsSorted(output_0));
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(output_1.origins.size(), 1);
ASSERT_EQ(output_1.ranges.size(), kNumSamples);
Expand All @@ -96,7 +108,7 @@ TEST(RangeDataCollatorTest, SingleSensorEmptyData) {
common::FromSeconds(output_1.ranges[0].point_time.time)),
300, 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(output_2.origins.size(), 1);
EXPECT_EQ(output_2.ranges.size(), 1);
Expand All @@ -109,10 +121,10 @@ TEST(RangeDataCollatorTest, TwoSensors) {
const std::string sensor_1 = "sensor_1";
RangeDataCollator collator({sensor_0, sensor_1});
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);
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(common::ToUniversal(output_1.time), 300);
ASSERT_EQ(output_1.ranges.size(), 2 * kNumSamples - 1);
Expand All @@ -123,7 +135,7 @@ TEST(RangeDataCollatorTest, TwoSensors) {
EXPECT_EQ(output_1.ranges.back().point_time.time, 0.f);
EXPECT_TRUE(ArePointTimestampsSorted(output_1));
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(common::ToUniversal(output_2.time), 310);
ASSERT_EQ(output_2.ranges.size(), 2);
Expand All @@ -135,7 +147,7 @@ TEST(RangeDataCollatorTest, TwoSensors) {
EXPECT_TRUE(ArePointTimestampsSorted(output_2));
// Sending the same sensor will flush everything before.
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(
output_1.ranges.size() + output_2.ranges.size() + output_3.ranges.size(),
Expand All @@ -150,19 +162,42 @@ TEST(RangeDataCollatorTest, ThreeSensors) {
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));
collator.AddRangeData(sensor_0, CreateFakeRangeData(100, 200, false));
EXPECT_EQ(output_0.ranges.size(), 0);
auto output_1 =
collator.AddRangeData(sensor_1, CreateFakeRangeData(199, 250, false));
EXPECT_EQ(output_1.ranges.size(), 0);
auto output_2 =
collator.AddRangeData(sensor_2, CreateFakeRangeData(210, 300, false));
EXPECT_EQ(output_2.ranges.size(), kNumSamples + 1);
EXPECT_TRUE(ArePointTimestampsSorted(output_2));
auto output_3 =
collator.AddRangeData(sensor_2, CreateFakeRangeData(400, 500, false));
EXPECT_EQ(output_2.ranges.size() + output_3.ranges.size(), 3 * kNumSamples);
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));
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));
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));
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
Expand Down
1 change: 1 addition & 0 deletions cartographer/sensor/proto/sensor.proto
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,7 @@ message TimedPointCloudData {
transform.proto.Vector3f origin = 2;
repeated transform.proto.Vector4f point_data_legacy = 3;
repeated TimedRangefinderPoint point_data = 4;
repeated float intensities = 5;
}

// Proto representation of ::cartographer::sensor::RangeData.
Expand Down
9 changes: 8 additions & 1 deletion cartographer/sensor/timed_point_cloud_data.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,10 +31,15 @@ proto::TimedPointCloudData ToProto(
for (const TimedRangefinderPoint& range : timed_point_cloud_data.ranges) {
*proto.add_point_data() = ToProto(range);
}
for (const float intensity : timed_point_cloud_data.intensities) {
proto.add_intensities(intensity);
}
return proto;
}

TimedPointCloudData FromProto(const proto::TimedPointCloudData& proto) {
CHECK(proto.intensities().size() == 0 ||
proto.intensities().size() == proto.point_data().size());
TimedPointCloud timed_point_cloud;
if (proto.point_data().size() > 0) {
timed_point_cloud.reserve(proto.point_data().size());
Expand All @@ -50,7 +55,9 @@ TimedPointCloudData FromProto(const proto::TimedPointCloudData& proto) {
}
return TimedPointCloudData{common::FromUniversal(proto.timestamp()),
transform::ToEigen(proto.origin()),
timed_point_cloud};
timed_point_cloud,
std::vector<float>(proto.intensities().begin(),
proto.intensities().end())};
}

} // namespace sensor
Expand Down
3 changes: 3 additions & 0 deletions cartographer/sensor/timed_point_cloud_data.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,11 +28,14 @@ struct TimedPointCloudData {
common::Time time;
Eigen::Vector3f origin;
TimedPointCloud ranges;
// 'intensities' has to be same size as 'ranges', or empty.
std::vector<float> intensities;
};

struct TimedPointCloudOriginData {
struct RangeMeasurement {
TimedRangefinderPoint point_time;
float intensity;
size_t origin_index;
};
common::Time time;
Expand Down

0 comments on commit a20db75

Please sign in to comment.