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

Introduce [Timed]RangefinderPoint. (#1357)

master
Juraj Oršulić 2018-08-10 22:36:37 +02:00 committed by Alexander Belyaev
parent dcf63d6684
commit 73c3d477d7
62 changed files with 491 additions and 319 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
cartographer/cloud/internal/handlers/add_rangefinder_data_handler_test.cc
View File

@ -41,7 +41,10 @@ const std::string kMessage = R"(
x: 3.f y: 4.f z: 5.f
}
point_data {
x: 6.f y: 7.f z: 8.f t: 9.f
position {
x: 6.f y: 7.f z: 8.f
}
time: 9.f
}
})";

2
cartographer/io/min_max_range_filtering_points_processor.cc
View File

@ -40,7 +40,7 @@ void MinMaxRangeFiteringPointsProcessor::Process(
std::unique_ptr<PointsBatch> batch) {
std::unordered_set<int> to_remove;
for (size_t i = 0; i < batch->points.size(); ++i) {
const float range = (batch->points[i] - batch->origin).norm();
const float range = (batch->points[i].position - batch->origin).norm();
if (!(min_range_ <= range && range <= max_range_)) {
to_remove.insert(i);
}

7
cartographer/io/outlier_removing_points_processor.cc
View File

@ -81,7 +81,8 @@ PointsProcessor::FlushResult OutlierRemovingPointsProcessor::Flush() {
void OutlierRemovingPointsProcessor::ProcessInPhaseOne(
const PointsBatch& batch) {
for (size_t i = 0; i < batch.points.size(); ++i) {
++voxels_.mutable_value(voxels_.GetCellIndex(batch.points[i]))->hits;
++voxels_.mutable_value(voxels_.GetCellIndex(batch.points[i].position))
->hits;
}
}
@ -91,7 +92,7 @@ void OutlierRemovingPointsProcessor::ProcessInPhaseTwo(
// by better ray casting, and also by marking the hits of the current range
// data to be excluded.
for (size_t i = 0; i < batch.points.size(); ++i) {
const Eigen::Vector3f delta = batch.points[i] - batch.origin;
const Eigen::Vector3f delta = batch.points[i].position - batch.origin;
const float length = delta.norm();
for (float x = 0; x < length; x += voxel_size_) {
const Eigen::Array3i index =
@ -109,7 +110,7 @@ void OutlierRemovingPointsProcessor::ProcessInPhaseThree(
std::unordered_set<int> to_remove;
for (size_t i = 0; i < batch->points.size(); ++i) {
const VoxelData voxel =
voxels_.value(voxels_.GetCellIndex(batch->points[i]));
voxels_.value(voxels_.GetCellIndex(batch->points[i].position));
if (!(voxel.rays < kMissPerHitLimit * voxel.hits)) {
to_remove.insert(i);
}

3
cartographer/io/pcd_writing_points_processor.cc
View File

@ -119,7 +119,8 @@ void PcdWritingPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
WriteBinaryPcdHeader(has_colors_, 0, file_writer_.get());
}
for (size_t i = 0; i < batch->points.size(); ++i) {
WriteBinaryPcdPointCoordinate(batch->points[i], file_writer_.get());
WriteBinaryPcdPointCoordinate(batch->points[i].position,
file_writer_.get());
if (!batch->colors.empty()) {
WriteBinaryPcdPointColor(ToUint8Color(batch->colors[i]),
file_writer_.get());

2
cartographer/io/ply_writing_points_processor.cc
View File

@ -136,7 +136,7 @@ void PlyWritingPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
}
for (size_t i = 0; i < batch->points.size(); ++i) {
WriteBinaryPlyPointCoordinate(batch->points[i], file_.get());
WriteBinaryPlyPointCoordinate(batch->points[i].position, file_.get());
if (has_colors_) {
WriteBinaryPlyPointColor(ToUint8Color(batch->colors[i]), file_.get());
}

2
cartographer/io/points_batch.cc
View File

@ -21,7 +21,7 @@ namespace io {
void RemovePoints(std::unordered_set<int> to_remove, PointsBatch* batch) {
const int new_num_points = batch->points.size() - to_remove.size();
std::vector<Eigen::Vector3f> points;
sensor::PointCloud points;
points.reserve(new_num_points);
std::vector<float> intensities;
if (!batch->intensities.empty()) {

3
cartographer/io/points_batch.h
View File

@ -17,6 +17,7 @@
#ifndef CARTOGRAPHER_IO_POINTS_BATCH_H_
#define CARTOGRAPHER_IO_POINTS_BATCH_H_
#include <cartographer/sensor/point_cloud.h>
#include <array>
#include <cstdint>
#include <unordered_set>
@ -52,7 +53,7 @@ struct PointsBatch {
int trajectory_id;
// Geometry of the points in the map frame.
std::vector<Eigen::Vector3f> points;
sensor::PointCloud points;
// Intensities are optional and may be unspecified. The meaning of these
// intensity values varies by device. For example, the VLP16 provides values

10
cartographer/io/probability_grid_points_processor_test.cc
View File

@ -34,11 +34,11 @@ namespace {
std::unique_ptr<PointsBatch> CreatePointsBatch() {
auto points_batch = ::absl::make_unique<PointsBatch>();
points_batch->origin = Eigen::Vector3f(0, 0, 0);
points_batch->points.emplace_back(0.0f, 0.0f, 0.0f);
points_batch->points.emplace_back(0.0f, 1.0f, 2.0f);
points_batch->points.emplace_back(1.0f, 2.0f, 4.0f);
points_batch->points.emplace_back(0.0f, 3.0f, 5.0f);
points_batch->points.emplace_back(3.0f, 0.0f, 6.0f);
points_batch->points.push_back({Eigen::Vector3f{0.0f, 0.0f, 0.0f}});
points_batch->points.push_back({Eigen::Vector3f{0.0f, 1.0f, 2.0f}});
points_batch->points.push_back({Eigen::Vector3f{1.0f, 2.0f, 4.0f}});
points_batch->points.push_back({Eigen::Vector3f{0.0f, 3.0f, 5.0f}});
points_batch->points.push_back({Eigen::Vector3f{3.0f, 0.0f, 6.0f}});
return points_batch;
}

4
cartographer/io/xray_points_processor.cc
View File

@ -196,9 +196,9 @@ void XRayPointsProcessor::Insert(const PointsBatch& batch,
Aggregation* const aggregation) {
constexpr FloatColor kDefaultColor = {{0.f, 0.f, 0.f}};
for (size_t i = 0; i < batch.points.size(); ++i) {
const Eigen::Vector3f camera_point = transform_ * batch.points[i];
const sensor::RangefinderPoint camera_point = transform_ * batch.points[i];
const Eigen::Array3i cell_index =
aggregation->voxels.GetCellIndex(camera_point);
aggregation->voxels.GetCellIndex(camera_point.position);
*aggregation->voxels.mutable_value(cell_index) = true;
bounding_box_.extend(cell_index.matrix());
ColumnData& column_data =

4
cartographer/io/xyz_writing_points_processor.cc
View File

@ -48,8 +48,8 @@ PointsProcessor::FlushResult XyzWriterPointsProcessor::Flush() {
}
void XyzWriterPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
for (const Eigen::Vector3f& point : batch->points) {
WriteXyzPoint(point, file_writer_.get());
for (const sensor::RangefinderPoint& point : batch->points) {
WriteXyzPoint(point.position, file_writer_.get());
}
next_->Process(std::move(batch));
}

16
cartographer/mapping/2d/probability_grid_range_data_inserter_2d.cc
View File

@ -37,11 +37,11 @@ void GrowAsNeeded(const sensor::RangeData& range_data,
Eigen::AlignedBox2f bounding_box(range_data.origin.head<2>());
// Padding around bounding box to avoid numerical issues at cell boundaries.
constexpr float kPadding = 1e-6f;
for (const Eigen::Vector3f& hit : range_data.returns) {
bounding_box.extend(hit.head<2>());
for (const sensor::RangefinderPoint& hit : range_data.returns) {
bounding_box.extend(hit.position.head<2>());
}
for (const Eigen::Vector3f& miss : range_data.misses) {
bounding_box.extend(miss.head<2>());
for (const sensor::RangefinderPoint& miss : range_data.misses) {
bounding_box.extend(miss.position.head<2>());
}
probability_grid->GrowLimits(bounding_box.min() -
kPadding * Eigen::Vector2f::Ones());
@ -66,8 +66,8 @@ void CastRays(const sensor::RangeData& range_data,
// Compute and add the end points.
std::vector<Eigen::Array2i> ends;
ends.reserve(range_data.returns.size());
for (const Eigen::Vector3f& hit : range_data.returns) {
ends.push_back(superscaled_limits.GetCellIndex(hit.head<2>()));
for (const sensor::RangefinderPoint& hit : range_data.returns) {
ends.push_back(superscaled_limits.GetCellIndex(hit.position.head<2>()));
probability_grid->ApplyLookupTable(ends.back() / kSubpixelScale, hit_table);
}
@ -85,9 +85,9 @@ void CastRays(const sensor::RangeData& range_data,
}
// Finally, compute and add empty rays based on misses in the range data.
for (const Eigen::Vector3f& missing_echo : range_data.misses) {
for (const sensor::RangefinderPoint& missing_echo : range_data.misses) {
std::vector<Eigen::Array2i> ray = RayToPixelMask(
begin, superscaled_limits.GetCellIndex(missing_echo.head<2>()),
begin, superscaled_limits.GetCellIndex(missing_echo.position.head<2>()),
kSubpixelScale);
for (const Eigen::Array2i& cell_index : ray) {
probability_grid->ApplyLookupTable(cell_index, miss_table);

8
cartographer/mapping/2d/range_data_inserter_2d_test.cc
View File

@ -49,10 +49,10 @@ class RangeDataInserterTest2D : public ::testing::Test {
void InsertPointCloud() {
sensor::RangeData range_data;
range_data.returns.emplace_back(-3.5f, 0.5f, 0.f);
range_data.returns.emplace_back(-2.5f, 1.5f, 0.f);
range_data.returns.emplace_back(-1.5f, 2.5f, 0.f);
range_data.returns.emplace_back(-0.5f, 3.5f, 0.f);
range_data.returns.push_back({Eigen::Vector3f{-3.5f, 0.5f, 0.f}});
range_data.returns.push_back({Eigen::Vector3f{-2.5f, 1.5f, 0.f}});
range_data.returns.push_back({Eigen::Vector3f{-1.5f, 2.5f, 0.f}});
range_data.returns.push_back({Eigen::Vector3f{-0.5f, 3.5f, 0.f}});
range_data.origin.x() = -0.5f;
range_data.origin.y() = 0.5f;
range_data_inserter_->Insert(range_data, &probability_grid_);

20
cartographer/mapping/2d/tsdf_range_data_inserter_2d.cc
View File

@ -33,9 +33,11 @@ const float kSqrtTwoPi = std::sqrt(2.0 * M_PI);
void GrowAsNeeded(const sensor::RangeData& range_data,
const float truncation_distance, TSDF2D* const tsdf) {
Eigen::AlignedBox2f bounding_box(range_data.origin.head<2>());
for (const Eigen::Vector3f& hit : range_data.returns) {
const Eigen::Vector3f direction = (hit - range_data.origin).normalized();
const Eigen::Vector3f end_position = hit + truncation_distance * direction;
for (const sensor::RangefinderPoint& hit : range_data.returns) {
const Eigen::Vector3f direction =
(hit.position - range_data.origin).normalized();
const Eigen::Vector3f end_position =
hit.position + truncation_distance * direction;
bounding_box.extend(end_position.head<2>());
}
// Padding around bounding box to avoid numerical issues at cell boundaries.
@ -66,9 +68,12 @@ std::pair<Eigen::Array2i, Eigen::Array2i> SuperscaleRay(
struct RangeDataSorter {
RangeDataSorter(Eigen::Vector3f origin) { origin_ = origin.head<2>(); }
bool operator()(Eigen::Vector3f lhs, Eigen::Vector3f rhs) {
const Eigen::Vector2f delta_lhs = (lhs.head<2>() - origin_).normalized();
const Eigen::Vector2f delta_rhs = (lhs.head<2>() - origin_).normalized();
bool operator()(const sensor::RangefinderPoint& lhs,
const sensor::RangefinderPoint& rhs) {
const Eigen::Vector2f delta_lhs =
(lhs.position.head<2>() - origin_).normalized();
const Eigen::Vector2f delta_rhs =
(lhs.position.head<2>() - origin_).normalized();
if ((delta_lhs[1] < 0.f) != (delta_rhs[1] < 0.f)) {
return delta_lhs[1] < 0.f;
} else if (delta_lhs[1] < 0.f) {
@ -147,7 +152,8 @@ void TSDFRangeDataInserter2D::Insert(const sensor::RangeData& range_data,
const Eigen::Vector2f origin = sorted_range_data.origin.head<2>();
for (size_t hit_index = 0; hit_index < sorted_range_data.returns.size();
++hit_index) {
const Eigen::Vector2f hit = sorted_range_data.returns[hit_index].head<2>();
const Eigen::Vector2f hit =
sorted_range_data.returns[hit_index].position.head<2>();
const float normal = normals.empty()
? std::numeric_limits<float>::quiet_NaN()
: normals[hit_index];

16
cartographer/mapping/2d/tsdf_range_data_inserter_2d_test.cc
View File

@ -49,7 +49,7 @@ class RangeDataInserterTest2DTSDF : public ::testing::Test {
void InsertPoint() {
sensor::RangeData range_data;
range_data.returns.emplace_back(-0.5f, 3.5f, 0.f);
range_data.returns.push_back({Eigen::Vector3f{-0.5f, 3.5f, 0.f}});
range_data.origin.x() = -0.5f;
range_data.origin.y() = -0.5f;
range_data_inserter_->Insert(range_data, &tsdf_);
@ -237,9 +237,9 @@ TEST_F(RangeDataInserterTest2DTSDF, InsertPointQuadraticWeight) {
TEST_F(RangeDataInserterTest2DTSDF,
InsertSmallAnglePointWithoutNormalProjection) {
sensor::RangeData range_data;
range_data.returns.emplace_back(-0.5f, 3.5f, 0.f);
range_data.returns.emplace_back(5.5f, 3.5f, 0.f);
range_data.returns.emplace_back(10.5f, 3.5f, 0.f);
range_data.returns.push_back({Eigen::Vector3f{-0.5f, 3.5f, 0.f}});
range_data.returns.push_back({Eigen::Vector3f{5.5f, 3.5f, 0.f}});
range_data.returns.push_back({Eigen::Vector3f{10.5f, 3.5f, 0.f}});
range_data.origin.x() = -0.5f;
range_data.origin.y() = -0.5f;
range_data_inserter_->Insert(range_data, &tsdf_);
@ -264,8 +264,8 @@ TEST_F(RangeDataInserterTest2DTSDF, InsertSmallAnglePointWitNormalProjection) {
options_.set_project_sdf_distance_to_scan_normal(true);
range_data_inserter_ = absl::make_unique<TSDFRangeDataInserter2D>(options_);
sensor::RangeData range_data;
range_data.returns.emplace_back(-0.5f, 3.5f, 0.f);
range_data.returns.emplace_back(5.5f, 3.5f, 0.f);
range_data.returns.push_back({Eigen::Vector3f{-0.5f, 3.5f, 0.f}});
range_data.returns.push_back({Eigen::Vector3f{5.5f, 3.5f, 0.f}});
range_data.origin.x() = -0.5f;
range_data.origin.y() = -0.5f;
range_data_inserter_->Insert(range_data, &tsdf_);
@ -293,8 +293,8 @@ TEST_F(RangeDataInserterTest2DTSDF,
options_.set_update_weight_angle_scan_normal_to_ray_kernel_bandwith(bandwith);
range_data_inserter_ = absl::make_unique<TSDFRangeDataInserter2D>(options_);
sensor::RangeData range_data;
range_data.returns.emplace_back(-0.5f, 3.5f, 0.f);
range_data.returns.emplace_back(5.5f, 3.5f, 0.f);
range_data.returns.push_back({Eigen::Vector3f{-0.5f, 3.5f, 0.f}});
range_data.returns.push_back({Eigen::Vector3f{5.5f, 3.5f, 0.f}});
range_data.origin.x() = -0.5f;
range_data.origin.y() = -0.5f;
range_data_inserter_->Insert(range_data, &tsdf_);

8
cartographer/mapping/3d/range_data_inserter_3d.cc
View File

@ -30,8 +30,8 @@ void InsertMissesIntoGrid(const std::vector<uint16>& miss_table,
HybridGrid* hybrid_grid,
const int num_free_space_voxels) {
const Eigen::Array3i origin_cell = hybrid_grid->GetCellIndex(origin);
for (const Eigen::Vector3f& hit : returns) {
const Eigen::Array3i hit_cell = hybrid_grid->GetCellIndex(hit);
for (const sensor::RangefinderPoint& hit : returns) {
const Eigen::Array3i hit_cell = hybrid_grid->GetCellIndex(hit.position);
const Eigen::Array3i delta = hit_cell - origin_cell;
const int num_samples = delta.cwiseAbs().maxCoeff();
@ -79,8 +79,8 @@ void RangeDataInserter3D::Insert(const sensor::RangeData& range_data,
HybridGrid* hybrid_grid) const {
CHECK_NOTNULL(hybrid_grid);
for (const Eigen::Vector3f& hit : range_data.returns) {
const Eigen::Array3i hit_cell = hybrid_grid->GetCellIndex(hit);
for (const sensor::RangefinderPoint& hit : range_data.returns) {
const Eigen::Array3i hit_cell = hybrid_grid->GetCellIndex(hit.position);
hybrid_grid->ApplyLookupTable(hit_cell, hit_table_);
}

6
cartographer/mapping/3d/range_data_inserter_3d_test.cc
View File

@ -41,8 +41,10 @@ class RangeDataInserter3DTest : public ::testing::Test {
void InsertPointCloud() {
const Eigen::Vector3f origin = Eigen::Vector3f(0.f, 0.f, -4.f);
sensor::PointCloud returns = {
{-3.f, -1.f, 4.f}, {-2.f, 0.f, 4.f}, {-1.f, 1.f, 4.f}, {0.f, 2.f, 4.f}};
sensor::PointCloud returns = {{Eigen::Vector3f{-3.f, -1.f, 4.f}},
{Eigen::Vector3f{-2.f, 0.f, 4.f}},
{Eigen::Vector3f{-1.f, 1.f, 4.f}},
{Eigen::Vector3f{0.f, 2.f, 4.f}}};
range_data_inserter_->Insert(sensor::RangeData{origin, returns, {}},
&hybrid_grid_);
}

4
cartographer/mapping/3d/submap_3d.cc
View File

@ -41,8 +41,8 @@ struct PixelData {
sensor::RangeData FilterRangeDataByMaxRange(const sensor::RangeData& range_data,
const float max_range) {
sensor::RangeData result{range_data.origin, {}, {}};
for (const Eigen::Vector3f& hit : range_data.returns) {
if ((hit - range_data.origin).norm() <= max_range) {
for (const sensor::RangefinderPoint& hit : range_data.returns) {
if ((hit.position - range_data.origin).norm() <= max_range) {
result.returns.push_back(hit);
}
}

19
cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc
View File

@ -130,10 +130,10 @@ LocalTrajectoryBuilder2D::AddRangeData(
CHECK(!synchronized_data.ranges.empty());
// TODO(gaschler): Check if this can strictly be 0.
CHECK_LE(synchronized_data.ranges.back().point_time[3], 0.f);
CHECK_LE(synchronized_data.ranges.back().point_time.time, 0.f);
const common::Time time_first_point =
time +
common::FromSeconds(synchronized_data.ranges.front().point_time[3]);
common::FromSeconds(synchronized_data.ranges.front().point_time.time);
if (time_first_point < extrapolator_->GetLastPoseTime()) {
LOG(INFO) << "Extrapolator is still initializing.";
return nullptr;
@ -143,7 +143,7 @@ LocalTrajectoryBuilder2D::AddRangeData(
range_data_poses.reserve(synchronized_data.ranges.size());
bool warned = false;
for (const auto& range : synchronized_data.ranges) {
common::Time time_point = time + common::FromSeconds(range.point_time[3]);
common::Time time_point = time + common::FromSeconds(range.point_time.time);
if (time_point < extrapolator_->GetLastExtrapolatedTime()) {
if (!warned) {
LOG(ERROR)
@ -166,20 +166,23 @@ LocalTrajectoryBuilder2D::AddRangeData(
// Drop any returns below the minimum range and convert returns beyond the
// maximum range into misses.
for (size_t i = 0; i < synchronized_data.ranges.size(); ++i) {
const Eigen::Vector4f& hit = synchronized_data.ranges[i].point_time;
const sensor::TimedRangefinderPoint& hit =
synchronized_data.ranges[i].point_time;
const Eigen::Vector3f origin_in_local =
range_data_poses[i] *
synchronized_data.origins.at(synchronized_data.ranges[i].origin_index);
const Eigen::Vector3f hit_in_local = range_data_poses[i] * hit.head<3>();
const Eigen::Vector3f delta = hit_in_local - origin_in_local;
sensor::RangefinderPoint hit_in_local =
range_data_poses[i] * sensor::ToRangefinderPoint(hit);
const Eigen::Vector3f delta = hit_in_local.position - origin_in_local;
const float range = delta.norm();
if (range >= options_.min_range()) {
if (range <= options_.max_range()) {
accumulated_range_data_.returns.push_back(hit_in_local);
} else {
accumulated_range_data_.misses.push_back(
hit_in_local.position =
origin_in_local +
options_.missing_data_ray_length() / range * delta);
options_.missing_data_ray_length() / range * delta;
accumulated_range_data_.misses.push_back(hit_in_local);
}
}
}

12
cartographer/mapping/internal/2d/normal_estimation_2d.cc
View File

@ -32,7 +32,8 @@ float EstimateNormal(const sensor::PointCloud& returns,
const size_t sample_window_begin,
const size_t sample_window_end,
const Eigen::Vector3f& sensor_origin) {
const Eigen::Vector3f& estimation_point = returns[estimation_point_index];
const Eigen::Vector3f& estimation_point =
returns[estimation_point_index].position;
if (sample_window_end - sample_window_begin < 2) {
return NormalTo2DAngle(sensor_origin - estimation_point);
}
@ -42,7 +43,7 @@ float EstimateNormal(const sensor::PointCloud& returns,
for (size_t sample_point_index = sample_window_begin;
sample_point_index < sample_window_end; ++sample_point_index) {
if (sample_point_index == estimation_point_index) continue;
const Eigen::Vector3f& sample_point = returns[sample_point_index];
const Eigen::Vector3f& sample_point = returns[sample_point_index].position;
const Eigen::Vector3f& tangent = estimation_point - sample_point;
Eigen::Vector3f sample_normal = {-tangent[1], tangent[0], 0.f};
constexpr float kMinNormalLength = 1e-6f;
@ -83,11 +84,11 @@ std::vector<float> EstimateNormals(
const float sample_radius = normal_estimation_options.sample_radius();
for (size_t current_point = 0; current_point < range_data.returns.size();
++current_point) {
const Eigen::Vector3f& hit = range_data.returns[current_point];
const Eigen::Vector3f& hit = range_data.returns[current_point].position;
size_t sample_window_begin = current_point;
for (; sample_window_begin > 0 &&
current_point - sample_window_begin < max_num_samples / 2 &&
(hit - range_data.returns[sample_window_begin - 1]).norm() <
(hit - range_data.returns[sample_window_begin - 1].position).norm() <
sample_radius;
--sample_window_begin) {
}
@ -95,7 +96,8 @@ std::vector<float> EstimateNormals(
for (;
sample_window_end < range_data.returns.size() &&
sample_window_end - current_point < ceil(max_num_samples / 2.0) + 1 &&
(hit - range_data.returns[sample_window_end]).norm() < sample_radius;
(hit - range_data.returns[sample_window_end].position).norm() <
sample_radius;
++sample_window_end) {
}
const float normal_estimate =

30
cartographer/mapping/internal/2d/normal_estimation_2d_test.cc
View File

@ -46,7 +46,8 @@ TEST(NormalEstimation2DTest, SinglePoint) {
static_cast<double>(num_angles) * 2. * M_PI -
M_PI;
range_data.returns.clear();
range_data.returns.emplace_back(std::cos(angle), std::sin(angle), 0.f);
range_data.returns.push_back(
{Eigen::Vector3d{std::cos(angle), std::sin(angle), 0.}.cast<float>()});
std::vector<float> normals;
normals = EstimateNormals(range_data, options);
EXPECT_NEAR(common::NormalizeAngleDifference(angle - normals[0] - M_PI),
@ -63,9 +64,9 @@ TEST(NormalEstimation2DTest, StraightLineGeometry) {
const proto::NormalEstimationOptions2D options =
CreateNormalEstimationOptions2D(parameter_dictionary.get());
sensor::RangeData range_data;
range_data.returns.emplace_back(-1.f, 1.f, 0.f);
range_data.returns.emplace_back(0.f, 1.f, 0.f);
range_data.returns.emplace_back(1.f, 1.f, 0.f);
range_data.returns.push_back({Eigen::Vector3f{-1.f, 1.f, 0.f}});
range_data.returns.push_back({Eigen::Vector3f{0.f, 1.f, 0.f}});
range_data.returns.push_back({Eigen::Vector3f{1.f, 1.f, 0.f}});
range_data.origin.x() = 0.f;
range_data.origin.y() = 0.f;
std::vector<float> normals;
@ -75,9 +76,9 @@ TEST(NormalEstimation2DTest, StraightLineGeometry) {
}
normals.clear();
range_data.returns.clear();
range_data.returns.emplace_back(1.f, 1.f, 0.f);
range_data.returns.emplace_back(1.f, 0.f, 0.f);
range_data.returns.emplace_back(1.f, -1.f, 0.f);
range_data.returns.push_back({Eigen::Vector3f{1.f, 1.f, 0.f}});
range_data.returns.push_back({Eigen::Vector3f{1.f, 0.f, 0.f}});
range_data.returns.push_back({Eigen::Vector3f{1.f, -1.f, 0.f}});
normals = EstimateNormals(range_data, options);
for (const float normal : normals) {
EXPECT_NEAR(std::abs(normal), M_PI, 1e-4);
@ -85,9 +86,9 @@ TEST(NormalEstimation2DTest, StraightLineGeometry) {
normals.clear();
range_data.returns.clear();
range_data.returns.emplace_back(1.f, -1.f, 0.f);
range_data.returns.emplace_back(0.f, -1.f, 0.f);
range_data.returns.emplace_back(-1.f, -1.f, 0.f);
range_data.returns.push_back({Eigen::Vector3f{1.f, -1.f, 0.f}});
range_data.returns.push_back({Eigen::Vector3f{0.f, -1.f, 0.f}});
range_data.returns.push_back({Eigen::Vector3f{-1.f, -1.f, 0.f}});
normals = EstimateNormals(range_data, options);
for (const float normal : normals) {
EXPECT_NEAR(normal, M_PI_2, 1e-4);
@ -95,9 +96,9 @@ TEST(NormalEstimation2DTest, StraightLineGeometry) {
normals.clear();
range_data.returns.clear();
range_data.returns.emplace_back(-1.f, -1.f, 0.f);
range_data.returns.emplace_back(-1.f, 0.f, 0.f);
range_data.returns.emplace_back(-1.f, 1.f, 0.f);
range_data.returns.push_back({Eigen::Vector3f{-1.f, -1.f, 0.f}});
range_data.returns.push_back({Eigen::Vector3f{-1.f, 0.f, 0.f}});
range_data.returns.push_back({Eigen::Vector3f{-1.f, 1.f, 0.f}});
normals = EstimateNormals(range_data, options);
for (const float normal : normals) {
EXPECT_NEAR(normal, 0, 1e-4);
@ -122,7 +123,8 @@ TEST_P(CircularGeometry2DTest, NumSamplesPerNormal) {
const double angle = static_cast<double>(angle_idx) /
static_cast<double>(num_angles) * 2. * M_PI -
M_PI;
range_data.returns.emplace_back(std::cos(angle), std::sin(angle), 0.f);
range_data.returns.push_back(
{Eigen::Vector3d{std::cos(angle), std::sin(angle), 0.}.cast<float>()});
}
range_data.origin.x() = 0.f;
range_data.origin.y() = 0.f;

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

@ -44,7 +44,8 @@ class PoseGraph2DTest : public ::testing::Test {
// kMinProbability) to unknown space (== kMinProbability).
for (float t = 0.f; t < 2.f * M_PI; t += 0.005f) {
const float r = (std::sin(20.f * t) + 2.f) * std::sin(t + 2.f);
point_cloud_.emplace_back(r * std::sin(t), r * std::cos(t), 0.f);
point_cloud_.push_back(
{Eigen::Vector3f{r * std::sin(t), r * std::cos(t), 0.f}});
}
{

2
cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d_test.cc
View File

@ -42,7 +42,7 @@ class CeresScanMatcherTest : public ::testing::Test {
probability_grid_.limits().GetCellIndex(Eigen::Vector2f(-3.5f, 2.5f)),
kMaxProbability);
point_cloud_.emplace_back(-3.f, 2.f, 0.f);
point_cloud_.push_back({Eigen::Vector3f{-3.f, 2.f, 0.f}});
auto parameter_dictionary = common::MakeDictionary(R"text(
return {

8
cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_2d.cc
View File

@ -32,8 +32,8 @@ SearchParameters::SearchParameters(const double linear_search_window,
// We set this value to something on the order of resolution to make sure that
// the std::acos() below is defined.
float max_scan_range = 3.f * resolution;
for (const Eigen::Vector3f& point : point_cloud) {
const float range = point.head<2>().norm();
for (const sensor::RangefinderPoint& point : point_cloud) {
const float range = point.position.head<2>().norm();
max_scan_range = std::max(range, max_scan_range);
}
const double kSafetyMargin = 1. - 1e-3;
@ -116,9 +116,9 @@ std::vector<DiscreteScan2D> DiscretizeScans(
for (const sensor::PointCloud& scan : scans) {
discrete_scans.emplace_back();
discrete_scans.back().reserve(scan.size());
for (const Eigen::Vector3f& point : scan) {
for (const sensor::RangefinderPoint& point : scan) {
const Eigen::Vector2f translated_point =
Eigen::Affine2f(initial_translation) * point.head<2>();
Eigen::Affine2f(initial_translation) * point.position.head<2>();
discrete_scans.back().push_back(
map_limits.GetCellIndex(translated_point));
}

28
cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_test.cc
View File

@ -58,27 +58,27 @@ TEST(Candidate, Construction) {
TEST(GenerateRotatedScans, GenerateRotatedScans) {
sensor::PointCloud point_cloud;
point_cloud.emplace_back(-1.f, 1.f, 0.f);
point_cloud.push_back({Eigen::Vector3f{-1.f, 1.f, 0.f}});
const std::vector<sensor::PointCloud> scans =
GenerateRotatedScans(point_cloud, SearchParameters(0, 1, M_PI / 2., 0.));
EXPECT_EQ(3, scans.size());
EXPECT_NEAR(1., scans[0][0].x(), 1e-6);
EXPECT_NEAR(1., scans[0][0].y(), 1e-6);
EXPECT_NEAR(-1., scans[1][0].x(), 1e-6);
EXPECT_NEAR(1., scans[1][0].y(), 1e-6);
EXPECT_NEAR(-1., scans[2][0].x(), 1e-6);
EXPECT_NEAR(-1., scans[2][0].y(), 1e-6);
EXPECT_NEAR(1., scans[0][0].position.x(), 1e-6);
EXPECT_NEAR(1., scans[0][0].position.y(), 1e-6);
EXPECT_NEAR(-1., scans[1][0].position.x(), 1e-6);
EXPECT_NEAR(1., scans[1][0].position.y(), 1e-6);
EXPECT_NEAR(-1., scans[2][0].position.x(), 1e-6);
EXPECT_NEAR(-1., scans[2][0].position.y(), 1e-6);
}
TEST(DiscretizeScans, DiscretizeScans) {
sensor::PointCloud point_cloud;
point_cloud.emplace_back(0.025f, 0.175f, 0.f);
point_cloud.emplace_back(-0.025f, 0.175f, 0.f);
point_cloud.emplace_back(-0.075f, 0.175f, 0.f);
point_cloud.emplace_back(-0.125f, 0.175f, 0.f);
point_cloud.emplace_back(-0.125f, 0.125f, 0.f);
point_cloud.emplace_back(-0.125f, 0.075f, 0.f);
point_cloud.emplace_back(-0.125f, 0.025f, 0.f);
point_cloud.push_back({Eigen::Vector3f{0.025f, 0.175f, 0.f}});
point_cloud.push_back({Eigen::Vector3f{-0.025f, 0.175f, 0.f}});
point_cloud.push_back({Eigen::Vector3f{-0.075f, 0.175f, 0.f}});
point_cloud.push_back({Eigen::Vector3f{-0.125f, 0.175f, 0.f}});
point_cloud.push_back({Eigen::Vector3f{-0.125f, 0.125f, 0.f}});
point_cloud.push_back({Eigen::Vector3f{-0.125f, 0.075f, 0.f}});
point_cloud.push_back({Eigen::Vector3f{-0.125f, 0.025f, 0.f}});
const MapLimits map_limits(0.05, Eigen::Vector2d(0.05, 0.25),
CellLimits(6, 6));
const std::vector<sensor::PointCloud> scans =

24
cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc
View File

@ -150,12 +150,12 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
const auto options = CreateFastCorrelativeScanMatcherTestOptions2D(3);
sensor::PointCloud point_cloud;
point_cloud.emplace_back(-2.5f, 0.5f, 0.f);
point_cloud.emplace_back(-2.f, 0.5f, 0.f);
point_cloud.emplace_back(0.f, -0.5f, 0.f);
point_cloud.emplace_back(0.5f, -1.6f, 0.f);
point_cloud.emplace_back(2.5f, 0.5f, 0.f);
point_cloud.emplace_back(2.5f, 1.7f, 0.f);
point_cloud.push_back({Eigen::Vector3f{-2.5f, 0.5f, 0.f}});
point_cloud.push_back({Eigen::Vector3f{-2.f, 0.5f, 0.f}});
point_cloud.push_back({Eigen::Vector3f{0.f, -0.5f, 0.f}});
point_cloud.push_back({Eigen::Vector3f{0.5f, -1.6f, 0.f}});
point_cloud.push_back({Eigen::Vector3f{2.5f, 0.5f, 0.f}});
point_cloud.push_back({Eigen::Vector3f{2.5f, 1.7f, 0.f}});
for (int i = 0; i != 50; ++i) {
const transform::Rigid2f expected_pose(
@ -200,12 +200,12 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
const auto options = CreateFastCorrelativeScanMatcherTestOptions2D(6);
sensor::PointCloud unperturbed_point_cloud;
unperturbed_point_cloud.emplace_back(-2.5f, 0.5f, 0.f);
unperturbed_point_cloud.emplace_back(-2.25f, 0.5f, 0.f);
unperturbed_point_cloud.emplace_back(0.f, 0.5f, 0.f);
unperturbed_point_cloud.emplace_back(0.25f, 1.6f, 0.f);
unperturbed_point_cloud.emplace_back(2.5f, 0.5f, 0.f);
unperturbed_point_cloud.emplace_back(2.f, 1.8f, 0.f);
unperturbed_point_cloud.push_back({Eigen::Vector3f{-2.5f, 0.5f, 0.f}});
unperturbed_point_cloud.push_back({Eigen::Vector3f{-2.25f, 0.5f, 0.f}});
unperturbed_point_cloud.push_back({Eigen::Vector3f{0.f, 0.5f, 0.f}});
unperturbed_point_cloud.push_back({Eigen::Vector3f{0.25f, 1.6f, 0.f}});
unperturbed_point_cloud.push_back({Eigen::Vector3f{2.5f, 0.5f, 0.f}});
unperturbed_point_cloud.push_back({Eigen::Vector3f{2.f, 1.8f, 0.f}});
for (int i = 0; i != 20; ++i) {
const transform::Rigid2f perturbation(

5
cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.cc
View File

@ -49,8 +49,9 @@ class OccupiedSpaceCostFunction2D {
for (size_t i = 0; i < point_cloud_.size(); ++i) {
// Note that this is a 2D point. The third component is a scaling factor.
const Eigen::Matrix<T, 3, 1> point((T(point_cloud_[i].x())),
(T(point_cloud_[i].y())), T(1.));
const Eigen::Matrix<T, 3, 1> point((T(point_cloud_[i].position.x())),
(T(point_cloud_[i].position.y())),
T(1.));
const Eigen::Matrix<T, 3, 1> world = transform * point;
interpolator.Evaluate(
(limits.max().x() - world[0]) / limits.resolution() - 0.5 +

2
cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d_test.cc
View File

@ -34,7 +34,7 @@ TEST(OccupiedSpaceCostFunction2DTest, SmokeTest) {
ValueConversionTables conversion_tables;
ProbabilityGrid grid(MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)),
&conversion_tables);
sensor::PointCloud point_cloud = {Eigen::Vector3f{0.f, 0.f, 0.f}};
sensor::PointCloud point_cloud = {{Eigen::Vector3f{0.f, 0.f, 0.f}}};
ceres::Problem problem;
std::unique_ptr<ceres::CostFunction> cost_function(
CreateOccupiedSpaceCostFunction2D(1.f, point_cloud, grid));

14
cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc
View File

@ -52,13 +52,13 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
CreateProbabilityGridRangeDataInserterOptions2D(
parameter_dictionary.get()));
}
point_cloud_.emplace_back(0.025f, 0.175f, 0.f);
point_cloud_.emplace_back(-0.025f, 0.175f, 0.f);
point_cloud_.emplace_back(-0.075f, 0.175f, 0.f);
point_cloud_.emplace_back(-0.125f, 0.175f, 0.f);
point_cloud_.emplace_back(-0.125f, 0.125f, 0.f);
point_cloud_.emplace_back(-0.125f, 0.075f, 0.f);
point_cloud_.emplace_back(-0.125f, 0.025f, 0.f);
point_cloud_.push_back({Eigen::Vector3f{0.025f, 0.175f, 0.f}});
point_cloud_.push_back({Eigen::Vector3f{-0.025f, 0.175f, 0.f}});
point_cloud_.push_back({Eigen::Vector3f{-0.075f, 0.175f, 0.f}});
point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.175f, 0.f}});
point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.125f, 0.f}});
point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.075f, 0.f}});
point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.025f, 0.f}});
range_data_inserter_->Insert(
sensor::RangeData{Eigen::Vector3f::Zero(), point_cloud_, {}},
&probability_grid_);

5
cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d.cc
View File

@ -47,8 +47,9 @@ class TSDFMatchCostFunction2D {
T summed_weight = T(0);
for (size_t i = 0; i < point_cloud_.size(); ++i) {
// Note that this is a 2D point. The third component is a scaling factor.
const Eigen::Matrix<T, 3, 1> point((T(point_cloud_[i].x())),
(T(point_cloud_[i].y())), T(1.));
const Eigen::Matrix<T, 3, 1> point((T(point_cloud_[i].position.x())),
(T(point_cloud_[i].position.y())),
T(1.));
const Eigen::Matrix<T, 3, 1> world = transform * point;
const T point_weight = interpolated_grid_.GetWeight(world[0], world[1]);
summed_weight += point_weight;

10
cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d_test.cc
View File

@ -58,7 +58,7 @@ class TSDFSpaceCostFunction2DTest : public ::testing::Test {
void InsertPointcloud() {
sensor::RangeData range_data;
for (float x = -.5; x < 0.5f; x += 0.1) {
range_data.returns.emplace_back(x, 1.0f, 0.f);
range_data.returns.push_back({Eigen::Vector3f{x, 1.0f, 0.f}});
}
range_data.origin.x() = -0.5f;
range_data.origin.y() = -0.5f;
@ -73,7 +73,7 @@ class TSDFSpaceCostFunction2DTest : public ::testing::Test {
};
TEST_F(TSDFSpaceCostFunction2DTest, MatchEmptyTSDF) {
const sensor::PointCloud matching_cloud = {Eigen::Vector3f{0.f, 0.f, 0.f}};
const sensor::PointCloud matching_cloud = {{Eigen::Vector3f{0.f, 0.f, 0.f}}};
std::unique_ptr<ceres::CostFunction> cost_function(
CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_));
const std::array<double, 3> pose_estimate{{0., 0., 0.}};
@ -89,7 +89,7 @@ TEST_F(TSDFSpaceCostFunction2DTest, MatchEmptyTSDF) {
TEST_F(TSDFSpaceCostFunction2DTest, ExactInitialPose) {
InsertPointcloud();
const sensor::PointCloud matching_cloud = {Eigen::Vector3f{0.f, 1.0f, 0.f}};
const sensor::PointCloud matching_cloud = {{Eigen::Vector3f{0.f, 1.0f, 0.f}}};
std::unique_ptr<ceres::CostFunction> cost_function(
CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_));
const std::array<double, 3> pose_estimate{{0., 0., 0.}};
@ -109,7 +109,7 @@ TEST_F(TSDFSpaceCostFunction2DTest, ExactInitialPose) {
TEST_F(TSDFSpaceCostFunction2DTest, PertubatedInitialPose) {
InsertPointcloud();
sensor::PointCloud matching_cloud = {Eigen::Vector3f{0.f, 1.0f, 0.f}};
sensor::PointCloud matching_cloud = {{Eigen::Vector3f{0.f, 1.0f, 0.f}}};
std::unique_ptr<ceres::CostFunction> cost_function(
CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_));
std::array<double, 3> pose_estimate{{0., 0.1, 0.}};
@ -140,7 +140,7 @@ TEST_F(TSDFSpaceCostFunction2DTest, PertubatedInitialPose) {
TEST_F(TSDFSpaceCostFunction2DTest, InvalidInitialPose) {
InsertPointcloud();
sensor::PointCloud matching_cloud = {Eigen::Vector3f{0.f, 1.0f, 0.f}};
sensor::PointCloud matching_cloud = {{Eigen::Vector3f{0.f, 1.0f, 0.f}}};
std::unique_ptr<ceres::CostFunction> cost_function(
CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_));
std::array<double, 3> pose_estimate{{0., 0.4, 0.}};

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

@ -137,10 +137,10 @@ LocalTrajectoryBuilder3D::AddRangeData(
}
CHECK(!synchronized_data.ranges.empty());
CHECK_LE(synchronized_data.ranges.back().point_time[3], 0.f);
CHECK_LE(synchronized_data.ranges.back().point_time.time, 0.f);
const common::Time time_first_point =
current_sensor_time +
common::FromSeconds(synchronized_data.ranges.front().point_time[3]);
common::FromSeconds(synchronized_data.ranges.front().point_time.time);
if (time_first_point < extrapolator_->GetLastPoseTime()) {
LOG(INFO) << "Extrapolator is still initializing.";
return nullptr;
@ -156,7 +156,7 @@ LocalTrajectoryBuilder3D::AddRangeData(
for (const auto& hit : hits) {
common::Time time_point =
current_sensor_time + common::FromSeconds(hit.point_time[3]);
current_sensor_time + common::FromSeconds(hit.point_time.time);
if (time_point < extrapolator_->GetLastExtrapolatedTime()) {
if (!warned) {
LOG(ERROR)
@ -176,11 +176,11 @@ LocalTrajectoryBuilder3D::AddRangeData(
}
for (size_t i = 0; i < hits.size(); ++i) {
const Eigen::Vector3f hit_in_local =
hits_poses[i] * hits[i].point_time.head<3>();
sensor::RangefinderPoint hit_in_local =
hits_poses[i] * sensor::ToRangefinderPoint(hits[i].point_time);
const Eigen::Vector3f origin_in_local =
hits_poses[i] * synchronized_data.origins.at(hits[i].origin_index);
const Eigen::Vector3f delta = hit_in_local - origin_in_local;
const Eigen::Vector3f delta = hit_in_local.position - origin_in_local;
const float range = delta.norm();
if (range >= options_.min_range()) {
if (range <= options_.max_range()) {
@ -189,8 +189,9 @@ LocalTrajectoryBuilder3D::AddRangeData(
// 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(
origin_in_local + options_.max_range() / range * delta);
hit_in_local.position =
origin_in_local + options_.max_range() / range * delta;
accumulated_range_data_.misses.push_back(hit_in_local);
}
}
}

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

@ -179,38 +179,39 @@ class LocalTrajectoryBuilderTest : public ::testing::Test {
sensor::TimedPointCloud directions_in_rangefinder_frame;
for (int r = -8; r != 8; ++r) {
for (int s = -250; s != 250; ++s) {
Eigen::Vector4f first_point;
first_point << Eigen::AngleAxisf(M_PI * s / 250.,
Eigen::Vector3f::UnitZ()) *
const sensor::TimedRangefinderPoint first_point{
Eigen::Vector3f{
Eigen::AngleAxisf(M_PI * s / 250., Eigen::Vector3f::UnitZ()) *
Eigen::AngleAxisf(M_PI / 12. * r / 8.,
Eigen::Vector3f::UnitY()) *
Eigen::Vector3f::UnitX(),
0.;
Eigen::Vector3f::UnitX()},
0.};
directions_in_rangefinder_frame.push_back(first_point);
// Second orthogonal rangefinder.
Eigen::Vector4f second_point;
second_point << Eigen::AngleAxisf(M_PI / 2., Eigen::Vector3f::UnitX()) *
Eigen::AngleAxisf(M_PI * s / 250.,
Eigen::Vector3f::UnitZ()) *
const sensor::TimedRangefinderPoint second_point{
Eigen::Vector3f{
Eigen::AngleAxisf(M_PI / 2., Eigen::Vector3f::UnitX()) *
Eigen::AngleAxisf(M_PI * s / 250., Eigen::Vector3f::UnitZ()) *
Eigen::AngleAxisf(M_PI / 12. * r / 8.,
Eigen::Vector3f::UnitY()) *
Eigen::Vector3f::UnitX(),
0.;
Eigen::Vector3f::UnitX()},
0.};
directions_in_rangefinder_frame.push_back(second_point);
}
}
// We simulate a 30 m edge length box around the origin, also containing
// 50 cm radius spheres.
sensor::TimedPointCloud returns_in_world_frame;
for (const Eigen::Vector4f& direction_in_world_frame :
for (const auto& direction_in_world_frame :
sensor::TransformTimedPointCloud(directions_in_rangefinder_frame,
pose.cast<float>())) {
const Eigen::Vector3f origin =
pose.cast<float>() * Eigen::Vector3f::Zero();
Eigen::Vector4f return_point;
return_point << CollideWithBubbles(
origin, CollideWithBox(origin, direction_in_world_frame.head<3>())),
0.;
const sensor::TimedRangefinderPoint return_point{
CollideWithBubbles(
origin,
CollideWithBox(origin, direction_in_world_frame.position)),
0.};
returns_in_world_frame.push_back(return_point);
}
return {Eigen::Vector3f::Zero(),

2
cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d_test.cc
View File

@ -42,7 +42,7 @@ class CeresScanMatcher3DTest : public ::testing::Test {
Eigen::Vector3f(-5.f, 2.f, 0.f), Eigen::Vector3f(-6.f, 2.f, 0.f),
Eigen::Vector3f(-6.f, 3.f, 1.f), Eigen::Vector3f(-6.f, 4.f, 2.f),
Eigen::Vector3f(-7.f, 3.f, 1.f)}) {
point_cloud_.push_back(point);
point_cloud_.push_back({point});
hybrid_grid_.SetProbability(
hybrid_grid_.GetCellIndex(expected_pose_.cast<float>() * point), 1.);
}

13
cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.cc
View File

@ -167,9 +167,9 @@ FastCorrelativeScanMatcher3D::MatchFullSubmap(
const Eigen::Quaterniond& global_submap_rotation,
const TrajectoryNode::Data& constant_data, const float min_score) const {
float max_point_distance = 0.f;
for (const Eigen::Vector3f& point :
for (const sensor::RangefinderPoint& point :
constant_data.high_resolution_point_cloud) {
max_point_distance = std::max(max_point_distance, point.norm());
max_point_distance = std::max(max_point_distance, point.position.norm());
}
const int linear_window_size =
(width_in_voxels_ + 1) / 2 +
@ -223,9 +223,10 @@ DiscreteScan3D FastCorrelativeScanMatcher3D::DiscretizeScan(
const PrecomputationGrid3D& original_grid =
precomputation_grid_stack_->Get(0);
std::vector<Eigen::Array3i> full_resolution_cell_indices;
for (const Eigen::Vector3f& point :
for (const sensor::RangefinderPoint& point :
sensor::TransformPointCloud(point_cloud, pose)) {
full_resolution_cell_indices.push_back(original_grid.GetCellIndex(point));
full_resolution_cell_indices.push_back(
original_grid.GetCellIndex(point.position));
}
const int full_resolution_depth = std::min(options_.full_resolution_depth(),
options_.branch_and_bound_depth());
@ -271,8 +272,8 @@ std::vector<DiscreteScan3D> FastCorrelativeScanMatcher3D::GenerateDiscreteScans(
// We set this value to something on the order of resolution to make sure that
// the std::acos() below is defined.
float max_scan_range = 3.f * resolution_;
for (const Eigen::Vector3f& point : point_cloud) {
const float range = point.norm();
for (const sensor::RangefinderPoint& point : point_cloud) {
const float range = point.position.norm();
max_scan_range = std::max(range, max_scan_range);
}
const float kSafetyMargin = 1.f - 1e-2f;

17
cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc
View File

@ -41,12 +41,12 @@ class FastCorrelativeScanMatcher3DTest : public ::testing::Test {
void SetUp() override {
point_cloud_ = {
Eigen::Vector3f(4.f, 0.f, 0.f), Eigen::Vector3f(4.5f, 0.f, 0.f),
Eigen::Vector3f(5.f, 0.f, 0.f), Eigen::Vector3f(5.5f, 0.f, 0.f),
Eigen::Vector3f(0.f, 4.f, 0.f), Eigen::Vector3f(0.f, 4.5f, 0.f),
Eigen::Vector3f(0.f, 5.f, 0.f), Eigen::Vector3f(0.f, 5.5f, 0.f),
Eigen::Vector3f(0.f, 0.f, 4.f), Eigen::Vector3f(0.f, 0.f, 4.5f),
Eigen::Vector3f(0.f, 0.f, 5.f), Eigen::Vector3f(0.f, 0.f, 5.5f)};
{Eigen::Vector3f(4.f, 0.f, 0.f)}, {Eigen::Vector3f(4.5f, 0.f, 0.f)},
{Eigen::Vector3f(5.f, 0.f, 0.f)}, {Eigen::Vector3f(5.5f, 0.f, 0.f)},
{Eigen::Vector3f(0.f, 4.f, 0.f)}, {Eigen::Vector3f(0.f, 4.5f, 0.f)},
{Eigen::Vector3f(0.f, 5.f, 0.f)}, {Eigen::Vector3f(0.f, 5.5f, 0.f)},
{Eigen::Vector3f(0.f, 0.f, 4.f)}, {Eigen::Vector3f(0.f, 0.f, 4.5f)},
{Eigen::Vector3f(0.f, 0.f, 5.f)}, {Eigen::Vector3f(0.f, 0.f, 5.5f)}};
}
transform::Rigid3f GetRandomPose() {
@ -157,7 +157,8 @@ TEST_F(FastCorrelativeScanMatcher3DTest, CorrectPoseForMatch) {
const std::unique_ptr<FastCorrelativeScanMatcher3D::Result>
low_resolution_result = fast_correlative_scan_matcher->Match(
transform::Rigid3d::Identity(), transform::Rigid3d::Identity(),
CreateConstantData({Eigen::Vector3f(42.f, 42.f, 42.f)}), kMinScore);
CreateConstantData({{Eigen::Vector3f(42.f, 42.f, 42.f)}}),
kMinScore);
EXPECT_THAT(low_resolution_result, testing::IsNull())
<< low_resolution_result->low_resolution_score;
}
@ -185,7 +186,7 @@ TEST_F(FastCorrelativeScanMatcher3DTest, CorrectPoseForMatchFullSubmap) {
const std::unique_ptr<FastCorrelativeScanMatcher3D::Result>
low_resolution_result = fast_correlative_scan_matcher->MatchFullSubmap(
Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity(),
CreateConstantData({Eigen::Vector3f(42.f, 42.f, 42.f)}), kMinScore);
CreateConstantData({{Eigen::Vector3f(42.f, 42.f, 42.f)}}), kMinScore);
EXPECT_THAT(low_resolution_result, testing::IsNull())
<< low_resolution_result->low_resolution_score;
}

4
cartographer/mapping/internal/3d/scan_matching/low_resolution_matcher.cc
View File

@ -24,11 +24,11 @@ std::function<float(const transform::Rigid3f&)> CreateLowResolutionMatcher(
const HybridGrid* low_resolution_grid, const sensor::PointCloud* points) {
return [=](const transform::Rigid3f& pose) {
float score = 0.f;
for (const Eigen::Vector3f& point :
for (const sensor::RangefinderPoint& point :
sensor::TransformPointCloud(*points, pose)) {
// TODO(zhengj, whess): Interpolate the Grid to get better score.
score += low_resolution_grid->GetProbability(
low_resolution_grid->GetCellIndex(point));
low_resolution_grid->GetCellIndex(point.position));
}
return score / points->size();
};

2
cartographer/mapping/internal/3d/scan_matching/occupied_space_cost_function_3d.h
View File

@ -71,7 +71,7 @@ class OccupiedSpaceCostFunction3D {
T* const residual) const {
for (size_t i = 0; i < point_cloud_.size(); ++i) {
const Eigen::Matrix<T, 3, 1> world =
transform * point_cloud_[i].cast<T>();
transform * point_cloud_[i].position.cast<T>();
const T probability =
interpolated_grid_.GetProbability(world[0], world[1], world[2]);
residual[i] = scaling_factor_ * (1. - probability);

9
cartographer/mapping/internal/3d/scan_matching/real_time_correlative_scan_matcher_3d.cc
View File

@ -61,8 +61,8 @@ RealTimeCorrelativeScanMatcher3D::GenerateExhaustiveSearchTransforms(
// We set this value to something on the order of resolution to make sure that
// the std::acos() below is defined.
float max_scan_range = 3.f * resolution;
for (const Eigen::Vector3f& point : point_cloud) {
const float range = point.norm();
for (const sensor::RangefinderPoint& point : point_cloud) {
const float range = point.position.norm();
max_scan_range = std::max(range, max_scan_range);
}
const float kSafetyMargin = 1.f - 1e-3f;
@ -99,8 +99,9 @@ float RealTimeCorrelativeScanMatcher3D::ScoreCandidate(
const sensor::PointCloud& transformed_point_cloud,
const transform::Rigid3f& transform) const {
float score = 0.f;
for (const Eigen::Vector3f& point : transformed_point_cloud) {
score += hybrid_grid.GetProbability(hybrid_grid.GetCellIndex(point));
for (const sensor::RangefinderPoint& point : transformed_point_cloud) {
score +=
hybrid_grid.GetProbability(hybrid_grid.GetCellIndex(point.position));
}
score /= static_cast<float>(transformed_point_cloud.size());
const float angle = transform::GetAngle(transform);

2
cartographer/mapping/internal/3d/scan_matching/real_time_correlative_scan_matcher_3d_test.cc
View File

@ -44,7 +44,7 @@ class RealTimeCorrelativeScanMatcher3DTest : public ::testing::Test {
Eigen::Vector3f(-5.f, 2.f, 0.f), Eigen::Vector3f(-6.f, 2.f, 0.f),
Eigen::Vector3f(-6.f, 3.f, 1.f), Eigen::Vector3f(-6.f, 4.f, 2.f),
Eigen::Vector3f(-7.f, 3.f, 1.f)}) {
point_cloud_.push_back(point);
point_cloud_.push_back({point});
hybrid_grid_.SetProbability(
hybrid_grid_.GetCellIndex(expected_pose_.cast<float>() * point), 1.);
}

26
cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.cc
View File

@ -52,8 +52,8 @@ void AddValueToHistogram(float angle, const float value,
Eigen::Vector3f ComputeCentroid(const sensor::PointCloud& slice) {
CHECK(!slice.empty());
Eigen::Vector3f sum = Eigen::Vector3f::Zero();
for (const Eigen::Vector3f& point : slice) {
sum += point;
for (const sensor::RangefinderPoint& point : slice) {
sum += point.position;
}
return sum / static_cast<float>(slice.size());
}
@ -68,16 +68,17 @@ void AddPointCloudSliceToHistogram(const sensor::PointCloud& slice,
// will add the angle between points to the histogram with the maximum weight.
// This is to reject, e.g., the angles observed on the ceiling and floor.
const Eigen::Vector3f centroid = ComputeCentroid(slice);
Eigen::Vector3f last_point = slice.front();
for (const Eigen::Vector3f& point : slice) {
const Eigen::Vector2f delta = (point - last_point).head<2>();
const Eigen::Vector2f direction = (point - centroid).head<2>();
Eigen::Vector3f last_point_position = slice.front().position;
for (const sensor::RangefinderPoint& point : slice) {
const Eigen::Vector2f delta =
(point.position - last_point_position).head<2>();
const Eigen::Vector2f direction = (point.position - centroid).head<2>();
const float distance = delta.norm();
if (distance < kMinDistance || direction.norm() < kMinDistance) {
continue;
}
if (distance > kMaxDistance) {
last_point = point;
last_point_position = point.position;
continue;
}
const float angle = common::atan2(delta);
@ -97,13 +98,13 @@ sensor::PointCloud SortSlice(const sensor::PointCloud& slice) {
}
float angle;
Eigen::Vector3f point;
sensor::RangefinderPoint point;
};
const Eigen::Vector3f centroid = ComputeCentroid(slice);
std::vector<SortableAnglePointPair> by_angle;
by_angle.reserve(slice.size());
for (const Eigen::Vector3f& point : slice) {
const Eigen::Vector2f delta = (point - centroid).head<2>();
for (const sensor::RangefinderPoint& point : slice) {
const Eigen::Vector2f delta = (point.position - centroid).head<2>();
if (delta.norm() < kMinDistance) {
continue;
}
@ -155,8 +156,9 @@ Eigen::VectorXf RotationalScanMatcher::ComputeHistogram(
const sensor::PointCloud& point_cloud, const int histogram_size) {
Eigen::VectorXf histogram = Eigen::VectorXf::Zero(histogram_size);
std::map<int, sensor::PointCloud> slices;
for (const Eigen::Vector3f& point : point_cloud) {
slices[common::RoundToInt(point.z() / kSliceHeight)].push_back(point);
for (const sensor::RangefinderPoint& point : point_cloud) {
slices[common::RoundToInt(point.position.z() / kSliceHeight)].push_back(
point);
}
for (const auto& slice : slices) {
AddPointCloudSliceToHistogram(SortSlice(slice.second), &histogram);

2
cartographer/mapping/internal/constraints/constraint_builder_2d_test.cc
View File

@ -70,7 +70,7 @@ TEST_F(ConstraintBuilder2DTest, CallsBack) {
TEST_F(ConstraintBuilder2DTest, FindsConstraints) {
TrajectoryNode::Data node_data;
node_data.filtered_gravity_aligned_point_cloud.push_back(
Eigen::Vector3f(0.1, 0.2, 0.3));
{Eigen::Vector3f(0.1, 0.2, 0.3)});
node_data.gravity_alignment = Eigen::Quaterniond::Identity();
node_data.local_pose = transform::Rigid3d::Identity();
SubmapId submap_id{0, 1};

4
cartographer/mapping/internal/constraints/constraint_builder_3d_test.cc
View File

@ -75,9 +75,9 @@ TEST_F(ConstraintBuilder3DTest, FindsConstraints) {
auto node_data = std::make_shared<TrajectoryNode::Data>();
node_data->gravity_alignment = Eigen::Quaterniond::Identity();
node_data->high_resolution_point_cloud.push_back(
Eigen::Vector3f(0.1, 0.2, 0.3));
{Eigen::Vector3f(0.1, 0.2, 0.3)});
node_data->low_resolution_point_cloud.push_back(
Eigen::Vector3f(0.1, 0.2, 0.3));
{Eigen::Vector3f(0.1, 0.2, 0.3)});
node_data->rotational_scan_matcher_histogram = Eigen::VectorXf::Zero(3);
node_data->local_pose = transform::Rigid3d::Identity();
node.constant_data = node_data;

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

@ -63,13 +63,14 @@ sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge() {
auto overlap_begin = ranges.begin();
while (overlap_begin < ranges.end() &&
data.time + common::FromSeconds((*overlap_begin)[3]) <
data.time + common::FromSeconds((*overlap_begin).time) <
current_start_) {
++overlap_begin;
}
auto overlap_end = overlap_begin;
while (overlap_end < ranges.end() &&
data.time + common::FromSeconds((*overlap_end)[3]) <= current_end_) {
data.time + common::FromSeconds((*overlap_end).time) <=
current_end_) {
++overlap_end;
}
if (ranges.begin() < overlap_begin && !warned_for_dropped_points) {
@ -82,14 +83,15 @@ sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge() {
if (overlap_begin < overlap_end) {
std::size_t origin_index = result.origins.size();
result.origins.push_back(data.origin);
double time_correction = common::ToSeconds(data.time - current_end_);
const float time_correction =
static_cast<float>(common::ToSeconds(data.time - current_end_));
for (auto overlap_it = overlap_begin; overlap_it != overlap_end;
++overlap_it) {
sensor::TimedPointCloudOriginData::RangeMeasurement point{*overlap_it,
origin_index};
// current_end_ + point_time[3]_after == in_timestamp +
// point_time[3]_before
point.point_time[3] += time_correction;
point.point_time.time += time_correction;
result.ranges.push_back(point);
}
}
@ -110,7 +112,7 @@ sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge() {
std::sort(result.ranges.begin(), result.ranges.end(),
[](const sensor::TimedPointCloudOriginData::RangeMeasurement& a,
const sensor::TimedPointCloudOriginData::RangeMeasurement& b) {
return a.point_time[3] < b.point_time[3];
return a.point_time.time < b.point_time.time;
});
return result;
}

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

@ -29,13 +29,13 @@ const int kNumSamples = 10;
sensor::TimedPointCloudData CreateFakeRangeData(int from, int to) {
double duration = common::ToSeconds(common::FromUniversal(to) -
common::FromUniversal(from));
sensor::TimedPointCloudData result{common::FromUniversal(to),
Eigen::Vector3f(0., 1., 2.),
sensor::TimedPointCloud(kNumSamples)};
sensor::TimedPointCloudData result{
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);
double relative_time = (1.f - fraction) * -duration;
result.ranges[i] = Eigen::Vector4f(1., 2., 3., relative_time);
float relative_time = (1.f - fraction) * -duration;
result.ranges.push_back({Eigen::Vector3f{1., 2., 3.}, relative_time});
}
return result;
}
@ -44,7 +44,7 @@ bool ArePointTimestampsSorted(const sensor::TimedPointCloudOriginData& data) {
std::vector<float> timestamps;
timestamps.reserve(data.ranges.size());
for (const auto& range : data.ranges) {
timestamps.push_back(range.point_time[3]);
timestamps.push_back(range.point_time.time);
}
return std::is_sorted(timestamps.begin(), timestamps.end());
}
@ -66,14 +66,14 @@ TEST(RangeDataCollatorTest, SingleSensor) {
EXPECT_TRUE(ArePointTimestampsSorted(output_1));
EXPECT_NEAR(common::ToUniversal(
output_1.time +
common::FromSeconds(output_1.ranges[0].point_time[3])),
common::FromSeconds(output_1.ranges[0].point_time.time)),
300, 2);
auto output_2 =
collator.AddRangeData(sensor_id, CreateFakeRangeData(-1000, 510));
EXPECT_EQ(common::ToUniversal(output_2.time), 510);
EXPECT_EQ(output_2.origins.size(), 1);
EXPECT_EQ(output_2.ranges.size(), 1);
EXPECT_EQ(output_2.ranges[0].point_time[3], 0.f);
EXPECT_EQ(output_2.ranges[0].point_time.time, 0.f);
EXPECT_TRUE(ArePointTimestampsSorted(output_2));
}
@ -93,14 +93,14 @@ TEST(RangeDataCollatorTest, SingleSensorEmptyData) {
EXPECT_TRUE(ArePointTimestampsSorted(output_1));
EXPECT_NEAR(common::ToUniversal(
output_1.time +
common::FromSeconds(output_1.ranges[0].point_time[3])),
common::FromSeconds(output_1.ranges[0].point_time.time)),
300, 2);
auto output_2 =
collator.AddRangeData(sensor_id, CreateFakeRangeData(-1000, 510));
EXPECT_EQ(common::ToUniversal(output_2.time), 510);
EXPECT_EQ(output_2.origins.size(), 1);
EXPECT_EQ(output_2.ranges.size(), 1);
EXPECT_EQ(output_2.ranges[0].point_time[3], 0.f);
EXPECT_EQ(output_2.ranges[0].point_time.time, 0.f);
EXPECT_TRUE(ArePointTimestampsSorted(output_2));
}
@ -118,9 +118,9 @@ TEST(RangeDataCollatorTest, TwoSensors) {
ASSERT_EQ(output_1.ranges.size(), 2 * kNumSamples - 1);
EXPECT_NEAR(common::ToUniversal(
output_1.time +
common::FromSeconds(output_1.ranges[0].point_time[3])),
common::FromSeconds(output_1.ranges[0].point_time.time)),
-1000, 2);
EXPECT_EQ(output_1.ranges.back().point_time[3], 0.f);
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));
@ -129,9 +129,9 @@ TEST(RangeDataCollatorTest, TwoSensors) {
ASSERT_EQ(output_2.ranges.size(), 2);
EXPECT_NEAR(common::ToUniversal(
output_2.time +
common::FromSeconds(output_2.ranges[0].point_time[3])),
common::FromSeconds(output_2.ranges[0].point_time.time)),
300, 2);
EXPECT_EQ(output_2.ranges.back().point_time[3], 0.f);
EXPECT_EQ(output_2.ranges.back().point_time.time, 0.f);
EXPECT_TRUE(ArePointTimestampsSorted(output_2));
// Sending the same sensor will flush everything before.
auto output_3 =
@ -140,7 +140,7 @@ TEST(RangeDataCollatorTest, TwoSensors) {
EXPECT_EQ(
output_1.ranges.size() + output_2.ranges.size() + output_3.ranges.size(),
3 * kNumSamples);
EXPECT_EQ(output_3.ranges.back().point_time[3], 0.f);
EXPECT_EQ(output_3.ranges.back().point_time.time, 0.f);
EXPECT_TRUE(ArePointTimestampsSorted(output_3));
}

6
cartographer/mapping/internal/testing/test_helpers.cc
View File

@ -56,8 +56,10 @@ GenerateFakeRangeMeasurements(const Eigen::Vector3f& translation,
for (double angle = 0.; angle < M_PI; angle += 0.01) {
for (double height : {-0.4, -0.2, 0.0, 0.2, 0.4}) {
constexpr double kRadius = 5;
point_cloud.emplace_back(kRadius * std::cos(angle),
kRadius * std::sin(angle), height, 0.);
point_cloud.push_back({Eigen::Vector3d{kRadius * std::cos(angle),
kRadius * std::sin(angle), height}
.cast<float>(),
0.});
}
}
const Eigen::Vector3f kVelocity = translation / duration;

9
cartographer/mapping/trajectory_node_test.cc
View File

@ -32,10 +32,13 @@ TEST(TrajectoryNodeTest, ToAndFromProto) {
const TrajectoryNode::Data expected{
common::FromUniversal(42),
Eigen::Quaterniond(1., 2., -3., -4.),
sensor::CompressedPointCloud({{1.f, 2.f, 0.f}, {0.f, 0.f, 1.f}})
sensor::CompressedPointCloud(
{{Eigen::Vector3f{1.f, 2.f, 0.f}}, {Eigen::Vector3f{0.f, 0.f, 1.f}}})
.Decompress(),
sensor::CompressedPointCloud({{Eigen::Vector3f{2.f, 3.f, 4.f}}})
.Decompress(),
sensor::CompressedPointCloud({{Eigen::Vector3f{-1.f, 2.f, 0.f}}})
.Decompress(),
sensor::CompressedPointCloud({{2.f, 3.f, 4.f}}).Decompress(),
sensor::CompressedPointCloud({{-1.f, 2.f, 0.f}}).Decompress(),
Eigen::VectorXf::Unit(20, 4),
transform::Rigid3d({1., 2., 3.},
Eigen::Quaterniond(4., 5., -6., -7.).normalized())};

18
cartographer/sensor/compressed_point_cloud.cc
View File

@ -56,9 +56,9 @@ CompressedPointCloud::ConstIterator::EndIterator(
return end_iterator;
}
Eigen::Vector3f CompressedPointCloud::ConstIterator::operator*() const {
RangefinderPoint CompressedPointCloud::ConstIterator::operator*() const {
CHECK_GT(remaining_points_, 0);
return current_point_;
return {current_point_};
}
CompressedPointCloud::ConstIterator& CompressedPointCloud::ConstIterator::
@ -109,14 +109,14 @@ CompressedPointCloud::CompressedPointCloud(const PointCloud& point_cloud)
CHECK_LE(point_cloud.size(), std::numeric_limits<int>::max());
for (int point_index = 0; point_index < static_cast<int>(point_cloud.size());
++point_index) {
const Eigen::Vector3f& point = point_cloud[point_index];
CHECK_LT(point.cwiseAbs().maxCoeff() / kPrecision,
const RangefinderPoint& point = point_cloud[point_index];
CHECK_LT(point.position.cwiseAbs().maxCoeff() / kPrecision,
1 << kMaxBitsPerDirection)
<< "Point out of bounds: " << point;
<< "Point out of bounds: " << point.position;
Eigen::Array3i raster_point;
Eigen::Array3i block_coordinate;
for (int i = 0; i < 3; ++i) {
raster_point[i] = common::RoundToInt(point[i] / kPrecision);
raster_point[i] = common::RoundToInt(point.position[i] / kPrecision);
block_coordinate[i] = raster_point[i] >> kBitsPerCoordinate;
raster_point[i] &= kCoordinateMask;
}
@ -170,14 +170,14 @@ CompressedPointCloud::ConstIterator CompressedPointCloud::end() const {
PointCloud CompressedPointCloud::Decompress() const {
PointCloud decompressed;
for (const Eigen::Vector3f& point : *this) {
for (const RangefinderPoint& point : *this) {
decompressed.push_back(point);
}
return decompressed;
}
bool sensor::CompressedPointCloud::operator==(
const sensor::CompressedPointCloud& right_hand_container) const {
bool CompressedPointCloud::operator==(
const CompressedPointCloud& right_hand_container) const {
return point_data_ == right_hand_container.point_data_ &&
num_points_ == right_hand_container.num_points_;
}

8
cartographer/sensor/compressed_point_cloud.h
View File

@ -61,10 +61,10 @@ class CompressedPointCloud {
class CompressedPointCloud::ConstIterator {
public:
using iterator_category = std::forward_iterator_tag;
using value_type = Eigen::Vector3f;
using value_type = RangefinderPoint;
using difference_type = int64;
using pointer = const Eigen::Vector3f*;
using reference = const Eigen::Vector3f&;
using pointer = const RangefinderPoint*;
using reference = const RangefinderPoint&;
// Creates begin iterator.
explicit ConstIterator(const CompressedPointCloud* compressed_point_cloud);
@ -73,7 +73,7 @@ class CompressedPointCloud::ConstIterator {
static ConstIterator EndIterator(
const CompressedPointCloud* compressed_point_cloud);
Eigen::Vector3f operator*() const;
RangefinderPoint operator*() const;
ConstIterator& operator++();
bool operator!=(const ConstIterator& it) const;

16
cartographer/sensor/compressed_point_cloud_test.cc
View File

@ -42,13 +42,13 @@ constexpr float kPrecision = 0.001f;
// Matcher for 3-d vectors w.r.t. to the target precision.
MATCHER_P(ApproximatelyEquals, expected,
std::string("is equal to ") + PrintToString(expected)) {
return (arg - expected).isZero(kPrecision);
return (arg.position - expected).isZero(kPrecision);
}
// Helper function to test the mapping of a single point. Includes test for
// recompressing the same point again.
void TestPoint(const Eigen::Vector3f& p) {
CompressedPointCloud compressed({p});
CompressedPointCloud compressed({{p}});
EXPECT_EQ(1, compressed.size());
EXPECT_THAT(*compressed.begin(), ApproximatelyEquals(p));
CompressedPointCloud recompressed({*compressed.begin()});
@ -70,9 +70,9 @@ TEST(CompressPointCloudTest, CompressesPointsCorrectly) {
}
TEST(CompressPointCloudTest, Compresses) {
const CompressedPointCloud compressed({Eigen::Vector3f(0.838f, 0, 0),
Eigen::Vector3f(0.839f, 0, 0),
Eigen::Vector3f(0.840f, 0, 0)});
const CompressedPointCloud compressed({{Eigen::Vector3f(0.838f, 0, 0)},
{Eigen::Vector3f(0.839f, 0, 0)},
{Eigen::Vector3f(0.840f, 0, 0)}});
EXPECT_FALSE(compressed.empty());
EXPECT_EQ(3, compressed.size());
const PointCloud decompressed = compressed.Decompress();
@ -98,7 +98,7 @@ TEST(CompressPointCloudTest, CompressesEmptyPointCloud) {
TEST(CompressPointCloudTest, CompressesNoGaps) {
PointCloud point_cloud;
for (int i = 0; i < 3000; ++i) {
point_cloud.push_back(Eigen::Vector3f(kPrecision * i - 1.5f, 0, 0));
point_cloud.push_back({Eigen::Vector3f(kPrecision * i - 1.5f, 0, 0)});
}
const CompressedPointCloud compressed(point_cloud);
const PointCloud decompressed = compressed.Decompress();
@ -106,8 +106,8 @@ TEST(CompressPointCloudTest, CompressesNoGaps) {
EXPECT_EQ(decompressed.size(), recompressed.size());
std::vector<float> x_coord;
for (const Eigen::Vector3f& p : compressed) {
x_coord.push_back(p[0]);
for (const RangefinderPoint& p : compressed) {
x_coord.push_back(p.position[0]);
}
std::sort(x_coord.begin(), x_coord.end());
for (size_t i = 1; i < x_coord.size(); ++i) {

3
cartographer/sensor/internal/ordered_multi_queue_test.cc
View File

@ -45,8 +45,7 @@ class OrderedMultiQueueTest : public ::testing::Test {
std::unique_ptr<Data> MakeImu(const int ordinal) {
return MakeDispatchable(
"imu",
sensor::ImuData{common::FromUniversal(ordinal), Eigen::Vector3d::Zero(),
"imu", ImuData{common::FromUniversal(ordinal), Eigen::Vector3d::Zero(),
Eigen::Vector3d::Zero()});
}

2
cartographer/sensor/internal/test_helpers.h
View File

@ -76,7 +76,7 @@ struct CollatorInput {
}
const int trajectory_id;
std::unique_ptr<sensor::Data> data;
std::unique_ptr<Data> data;
const CollatorOutput expected_output;
};

22
cartographer/sensor/internal/voxel_filter.cc
View File

@ -28,8 +28,8 @@ namespace {
PointCloud FilterByMaxRange(const PointCloud& point_cloud,
const float max_range) {
PointCloud result;
for (const Eigen::Vector3f& point : point_cloud) {
if (point.norm() <= max_range) {
for (const RangefinderPoint& point : point_cloud) {
if (point.position.norm() <= max_range) {
result.push_back(point);
}
}
@ -80,8 +80,9 @@ PointCloud AdaptivelyVoxelFiltered(
PointCloud VoxelFilter::Filter(const PointCloud& point_cloud) {
PointCloud results;
for (const Eigen::Vector3f& point : point_cloud) {
auto it_inserted = voxel_set_.insert(IndexToKey(GetCellIndex(point)));
for (const RangefinderPoint& point : point_cloud) {
auto it_inserted =
voxel_set_.insert(IndexToKey(GetCellIndex(point.position)));
if (it_inserted.second) {
results.push_back(point);
}
@ -91,9 +92,9 @@ PointCloud VoxelFilter::Filter(const PointCloud& point_cloud) {
TimedPointCloud VoxelFilter::Filter(const TimedPointCloud& timed_point_cloud) {
TimedPointCloud results;
for (const Eigen::Vector4f& point : timed_point_cloud) {
for (const TimedRangefinderPoint& point : timed_point_cloud) {
auto it_inserted =
voxel_set_.insert(IndexToKey(GetCellIndex(point.head<3>())));
voxel_set_.insert(IndexToKey(GetCellIndex(point.position)));
if (it_inserted.second) {
results.push_back(point);
}
@ -101,14 +102,13 @@ TimedPointCloud VoxelFilter::Filter(const TimedPointCloud& timed_point_cloud) {
return results;
}
std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement>
VoxelFilter::Filter(
const std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement>&
std::vector<TimedPointCloudOriginData::RangeMeasurement> VoxelFilter::Filter(
const std::vector<TimedPointCloudOriginData::RangeMeasurement>&
range_measurements) {
std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement> results;
std::vector<TimedPointCloudOriginData::RangeMeasurement> results;
for (const auto& range_measurement : range_measurements) {
auto it_inserted = voxel_set_.insert(
IndexToKey(GetCellIndex(range_measurement.point_time.head<3>())));
IndexToKey(GetCellIndex(range_measurement.point_time.position)));
if (it_inserted.second) {
results.push_back(range_measurement);
}

4
cartographer/sensor/internal/voxel_filter.h
View File

@ -46,8 +46,8 @@ class VoxelFilter {
TimedPointCloud Filter(const TimedPointCloud& timed_point_cloud);
// Same for RangeMeasurement.
std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement> Filter(
const std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement>&
std::vector<TimedPointCloudOriginData::RangeMeasurement> Filter(
const std::vector<TimedPointCloudOriginData::RangeMeasurement>&
range_measurements);
private:

18
cartographer/sensor/internal/voxel_filter_test.cc
View File

@ -27,19 +27,19 @@ namespace {
using ::testing::ContainerEq;
TEST(VoxelFilterTest, ReturnsTheFirstPointInEachVoxel) {
PointCloud point_cloud = {{0.f, 0.f, 0.f},
{0.1f, -0.1f, 0.1f},
{0.3f, -0.1f, 0.f},
{0.f, 0.f, 0.1f}};
PointCloud point_cloud = {{Eigen::Vector3f{0.f, 0.f, 0.f}},
{Eigen::Vector3f{0.1f, -0.1f, 0.1f}},
{Eigen::Vector3f{0.3f, -0.1f, 0.f}},
{Eigen::Vector3f{0.f, 0.f, 0.1f}}};
EXPECT_THAT(VoxelFilter(0.3f).Filter(point_cloud),
ContainerEq(PointCloud{point_cloud[0], point_cloud[2]}));
}
TEST(VoxelFilterTest, HandlesLargeCoordinates) {
PointCloud point_cloud = {{100000.f, 0.f, 0.f},
{100000.001f, -0.0001f, 0.0001f},
{100000.003f, -0.0001f, 0.f},
{-200000.f, 0.f, 0.f}};
PointCloud point_cloud = {{Eigen::Vector3f{100000.f, 0.f, 0.f}},
{Eigen::Vector3f{100000.001f, -0.0001f, 0.0001f}},
{Eigen::Vector3f{100000.003f, -0.0001f, 0.f}},
{Eigen::Vector3f{-200000.f, 0.f, 0.f}}};
EXPECT_THAT(VoxelFilter(0.01f).Filter(point_cloud),
ContainerEq(PointCloud{point_cloud[0], point_cloud[3]}));
}
@ -47,7 +47,7 @@ TEST(VoxelFilterTest, HandlesLargeCoordinates) {
TEST(VoxelFilterTest, IgnoresTime) {
TimedPointCloud timed_point_cloud;
for (int i = 0; i < 100; ++i) {
timed_point_cloud.emplace_back(-100.f, 0.3f, 0.4f, 1.f * i);
timed_point_cloud.push_back({Eigen::Vector3f{-100.f, 0.3f, 0.4f}, 1.f * i});
}
EXPECT_THAT(VoxelFilter(0.3f).Filter(timed_point_cloud),
ContainerEq(TimedPointCloud{timed_point_cloud[0]}));

17
cartographer/sensor/point_cloud.cc
View File

@ -26,7 +26,7 @@ PointCloud TransformPointCloud(const PointCloud& point_cloud,
const transform::Rigid3f& transform) {
PointCloud result;
result.reserve(point_cloud.size());
for (const Eigen::Vector3f& point : point_cloud) {
for (const RangefinderPoint& point : point_cloud) {
result.emplace_back(transform * point);
}
return result;
@ -36,11 +36,8 @@ TimedPointCloud TransformTimedPointCloud(const TimedPointCloud& point_cloud,
const transform::Rigid3f& transform) {
TimedPointCloud result;
result.reserve(point_cloud.size());
for (const Eigen::Vector4f& point : point_cloud) {
Eigen::Vector4f result_point;
result_point.head<3>() = transform * point.head<3>();
result_point[3] = point[3];
result.emplace_back(result_point);
for (const TimedRangefinderPoint& point : point_cloud) {
result.push_back(transform * point);
}
return result;
}
@ -48,8 +45,8 @@ TimedPointCloud TransformTimedPointCloud(const TimedPointCloud& point_cloud,
PointCloud CropPointCloud(const PointCloud& point_cloud, const float min_z,
const float max_z) {
PointCloud cropped_point_cloud;
for (const Eigen::Vector3f& point : point_cloud) {
if (min_z <= point.z() && point.z() <= max_z) {
for (const RangefinderPoint& point : point_cloud) {
if (min_z <= point.position.z() && point.position.z() <= max_z) {
cropped_point_cloud.push_back(point);
}
}
@ -59,8 +56,8 @@ PointCloud CropPointCloud(const PointCloud& point_cloud, const float min_z,
TimedPointCloud CropTimedPointCloud(const TimedPointCloud& point_cloud,
const float min_z, const float max_z) {
TimedPointCloud cropped_point_cloud;
for (const Eigen::Vector4f& point : point_cloud) {
if (min_z <= point.z() && point.z() <= max_z) {
for (const TimedRangefinderPoint& point : point_cloud) {
if (min_z <= point.position.z() && point.position.z() <= max_z) {
cropped_point_cloud.push_back(point);
}
}

5
cartographer/sensor/point_cloud.h
View File

@ -21,6 +21,7 @@
#include "Eigen/Core"
#include "cartographer/sensor/proto/sensor.pb.h"
#include "cartographer/sensor/rangefinder_point.h"
#include "cartographer/transform/rigid_transform.h"
#include "glog/logging.h"
@ -29,14 +30,14 @@ namespace sensor {
// Stores 3D positions of points.
// For 2D points, the third entry is 0.f.
typedef std::vector<Eigen::Vector3f> PointCloud;
using PointCloud = std::vector<RangefinderPoint>;
// Stores 3D positions of points with their relative measurement time in the
// fourth entry. Time is in seconds, increasing and relative to the moment when
// the last point was acquired. So, the fourth entry for the last point is 0.f.
// If timing is not available, all fourth entries are 0.f. For 2D points, the
// third entry is 0.f (and the fourth entry is time).
typedef std::vector<Eigen::Vector4f> TimedPointCloud;
using TimedPointCloud = std::vector<TimedRangefinderPoint>;
struct PointCloudWithIntensities {
TimedPointCloud points;

24
cartographer/sensor/point_cloud_test.cc
View File

@ -27,26 +27,26 @@ namespace {
TEST(PointCloudTest, TransformPointCloud) {
PointCloud point_cloud;
point_cloud.emplace_back(0.5f, 0.5f, 1.f);
point_cloud.emplace_back(3.5f, 0.5f, 42.f);
point_cloud.push_back({Eigen::Vector3f{0.5f, 0.5f, 1.f}});
point_cloud.push_back({Eigen::Vector3f{3.5f, 0.5f, 42.f}});
const PointCloud transformed_point_cloud = TransformPointCloud(
point_cloud, transform::Embed3D(transform::Rigid2f::Rotation(M_PI_2)));
EXPECT_NEAR(-0.5f, transformed_point_cloud[0].x(), 1e-6);
EXPECT_NEAR(0.5f, transformed_point_cloud[0].y(), 1e-6);
EXPECT_NEAR(-0.5f, transformed_point_cloud[1].x(), 1e-6);
EXPECT_NEAR(3.5f, transformed_point_cloud[1].y(), 1e-6);
EXPECT_NEAR(-0.5f, transformed_point_cloud[0].position.x(), 1e-6);
EXPECT_NEAR(0.5f, transformed_point_cloud[0].position.y(), 1e-6);
EXPECT_NEAR(-0.5f, transformed_point_cloud[1].position.x(), 1e-6);
EXPECT_NEAR(3.5f, transformed_point_cloud[1].position.y(), 1e-6);
}
TEST(PointCloudTest, TransformTimedPointCloud) {
TimedPointCloud point_cloud;
point_cloud.emplace_back(0.5f, 0.5f, 1.f, 0.f);
point_cloud.emplace_back(3.5f, 0.5f, 42.f, 0.f);
point_cloud.push_back({Eigen::Vector3f{0.5f, 0.5f, 1.f}, 0.f});
point_cloud.push_back({Eigen::Vector3f{3.5f, 0.5f, 42.f}, 0.f});
const TimedPointCloud transformed_point_cloud = TransformTimedPointCloud(
point_cloud, transform::Embed3D(transform::Rigid2f::Rotation(M_PI_2)));
EXPECT_NEAR(-0.5f, transformed_point_cloud[0].x(), 1e-6);
EXPECT_NEAR(0.5f, transformed_point_cloud[0].y(), 1e-6);
EXPECT_NEAR(-0.5f, transformed_point_cloud[1].x(), 1e-6);
EXPECT_NEAR(3.5f, transformed_point_cloud[1].y(), 1e-6);
EXPECT_NEAR(-0.5f, transformed_point_cloud[0].position.x(), 1e-6);
EXPECT_NEAR(0.5f, transformed_point_cloud[0].position.y(), 1e-6);
EXPECT_NEAR(-0.5f, transformed_point_cloud[1].position.x(), 1e-6);
EXPECT_NEAR(3.5f, transformed_point_cloud[1].position.y(), 1e-6);
}
} // namespace

18
cartographer/sensor/proto/sensor.proto
View File

@ -20,6 +20,15 @@ option java_outer_classname = "Sensor";
import "cartographer/transform/proto/transform.proto";
message RangefinderPoint {
transform.proto.Vector3f position = 1;
}
message TimedRangefinderPoint {
transform.proto.Vector3f position = 1;
float time = 2;
}
// Compressed collection of a 3D point cloud.
message CompressedPointCloud {
int32 num_points = 1;
@ -30,14 +39,17 @@ message CompressedPointCloud {
message TimedPointCloudData {
int64 timestamp = 1;
transform.proto.Vector3f origin = 2;
repeated transform.proto.Vector4f point_data = 3;
repeated transform.proto.Vector4f point_data_legacy = 3;
repeated TimedRangefinderPoint point_data = 4;
}
// Proto representation of ::cartographer::sensor::RangeData.
message RangeData {
transform.proto.Vector3f origin = 1;
repeated transform.proto.Vector3f returns = 2;
repeated transform.proto.Vector3f misses = 3;
repeated transform.proto.Vector3f returns_legacy = 2;
repeated transform.proto.Vector3f misses_legacy = 3;
repeated RangefinderPoint returns = 4;
repeated RangefinderPoint misses = 5;
}
// Proto representation of ::cartographer::sensor::ImuData.

37
cartographer/sensor/range_data.cc
View File

@ -58,30 +58,41 @@ proto::RangeData ToProto(const RangeData& range_data) {
proto::RangeData proto;
*proto.mutable_origin() = transform::ToProto(range_data.origin);
proto.mutable_returns()->Reserve(range_data.returns.size());
for (const Eigen::Vector3f& point : range_data.returns) {
*proto.add_returns() = transform::ToProto(point);
for (const RangefinderPoint& point : range_data.returns) {
*proto.add_returns() = ToProto(point);
}
proto.mutable_misses()->Reserve(range_data.misses.size());
for (const Eigen::Vector3f& point : range_data.misses) {
*proto.add_misses() = transform::ToProto(point);
for (const RangefinderPoint& point : range_data.misses) {
*proto.add_misses() = ToProto(point);
}
return proto;
}
RangeData FromProto(const proto::RangeData& proto) {
PointCloud returns;
if (proto.returns_size() > 0) {
returns.reserve(proto.returns().size());
std::transform(
proto.returns().begin(), proto.returns().end(),
std::back_inserter(returns),
static_cast<Eigen::Vector3f (*)(const transform::proto::Vector3f&)>(
transform::ToEigen));
for (const auto& point_proto : proto.returns()) {
returns.push_back(FromProto(point_proto));
}
} else {
returns.reserve(proto.returns_legacy().size());
for (const auto& point_proto : proto.returns_legacy()) {
returns.push_back({transform::ToEigen(point_proto)});
}
}
PointCloud misses;
if (proto.misses_size() > 0) {
misses.reserve(proto.misses().size());
std::transform(
proto.misses().begin(), proto.misses().end(), std::back_inserter(misses),
static_cast<Eigen::Vector3f (*)(const transform::proto::Vector3f&)>(
transform::ToEigen));
for (const auto& point_proto : proto.misses()) {
misses.push_back(FromProto(point_proto));
}
} else {
misses.reserve(proto.misses_legacy().size());
for (const auto& point_proto : proto.misses_legacy()) {
misses.push_back({transform::ToEigen(point_proto)});
}
}
return RangeData{transform::ToEigen(proto.origin()), returns, misses};
}

108
cartographer/sensor/rangefinder_point.h Normal file
View File

@ -0,0 +1,108 @@
/*
* Copyright 2018 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_SENSOR_RANGEFINDER_POINT_H_
#define CARTOGRAPHER_SENSOR_RANGEFINDER_POINT_H_
#include <vector>
#include "Eigen/Core"
#include "cartographer/sensor/proto/sensor.pb.h"
#include "cartographer/transform/transform.h"
#include "glog/logging.h"
namespace cartographer {
namespace sensor {
// Stores 3D position of a point observed by a rangefinder sensor.
struct RangefinderPoint {
Eigen::Vector3f position;
};
// Stores 3D position of a point with its relative measurement time.
// See point_cloud.h for more details.
struct TimedRangefinderPoint {
Eigen::Vector3f position;
float time;
};
template <class T>
inline RangefinderPoint operator*(const transform::Rigid3<T>& lhs,
const RangefinderPoint& rhs) {
RangefinderPoint result = rhs;
result.position = lhs * rhs.position;
return result;
}
template <class T>
inline TimedRangefinderPoint operator*(const transform::Rigid3<T>& lhs,
const TimedRangefinderPoint& rhs) {
TimedRangefinderPoint result = rhs;
result.position = lhs * rhs.position;
return result;
}
inline bool operator==(const RangefinderPoint& lhs,
const RangefinderPoint& rhs) {
return lhs.position == rhs.position;
}
inline bool operator==(const TimedRangefinderPoint& lhs,
const TimedRangefinderPoint& rhs) {
return lhs.position == rhs.position && lhs.time == rhs.time;
}
inline RangefinderPoint FromProto(
const proto::RangefinderPoint& rangefinder_point_proto) {
return {transform::ToEigen(rangefinder_point_proto.position())};
}
inline proto::RangefinderPoint ToProto(
const RangefinderPoint& rangefinder_point) {
proto::RangefinderPoint proto;
*proto.mutable_position() = transform::ToProto(rangefinder_point.position);
return proto;
}
inline TimedRangefinderPoint FromProto(
const proto::TimedRangefinderPoint& timed_rangefinder_point_proto) {
return {transform::ToEigen(timed_rangefinder_point_proto.position()),
timed_rangefinder_point_proto.time()};
}
inline proto::TimedRangefinderPoint ToProto(
const TimedRangefinderPoint& timed_rangefinder_point) {
proto::TimedRangefinderPoint proto;
*proto.mutable_position() =
transform::ToProto(timed_rangefinder_point.position);
proto.set_time(timed_rangefinder_point.time);
return proto;
}
inline RangefinderPoint ToRangefinderPoint(
const TimedRangefinderPoint& timed_rangefinder_point) {
return {timed_rangefinder_point.position};
}
inline TimedRangefinderPoint ToTimedRangefinderPoint(
const RangefinderPoint& rangefinder_point, const float time) {
return {rangefinder_point.position, time};
}
} // namespace sensor
} // namespace cartographer
#endif // CARTOGRAPHER_SENSOR_RANGEFINDER_POINT_H_

20
cartographer/sensor/timed_point_cloud_data.cc
View File

@ -28,20 +28,26 @@ proto::TimedPointCloudData ToProto(
proto.set_timestamp(common::ToUniversal(timed_point_cloud_data.time));
*proto.mutable_origin() = transform::ToProto(timed_point_cloud_data.origin);
proto.mutable_point_data()->Reserve(timed_point_cloud_data.ranges.size());
for (const Eigen::Vector4f& range : timed_point_cloud_data.ranges) {
*proto.add_point_data() = transform::ToProto(range);
for (const TimedRangefinderPoint& range : timed_point_cloud_data.ranges) {
*proto.add_point_data() = ToProto(range);
}
return proto;
}
TimedPointCloudData FromProto(const proto::TimedPointCloudData& proto) {
TimedPointCloud timed_point_cloud;
if (proto.point_data().size() > 0) {
timed_point_cloud.reserve(proto.point_data().size());
std::transform(
proto.point_data().begin(), proto.point_data().end(),
std::back_inserter(timed_point_cloud),
static_cast<Eigen::Vector4f (*)(const transform::proto::Vector4f&)>(
transform::ToEigen));
for (const auto& timed_point_proto : proto.point_data()) {
timed_point_cloud.push_back(FromProto(timed_point_proto));
}
} else {
timed_point_cloud.reserve(proto.point_data_legacy().size());
for (const auto& timed_point_proto : proto.point_data_legacy()) {
const Eigen::Vector4f timed_point = transform::ToEigen(timed_point_proto);
timed_point_cloud.push_back({timed_point.head<3>(), timed_point[3]});
}
}
return TimedPointCloudData{common::FromUniversal(proto.timestamp()),
transform::ToEigen(proto.origin()),
timed_point_cloud};

4
cartographer/sensor/timed_point_cloud_data.h
View File

@ -27,12 +27,12 @@ namespace sensor {
struct TimedPointCloudData {
common::Time time;
Eigen::Vector3f origin;
sensor::TimedPointCloud ranges;
TimedPointCloud ranges;
};
struct TimedPointCloudOriginData {
struct RangeMeasurement {
Eigen::Vector4f point_time;
TimedRangefinderPoint point_time;
size_t origin_index;
};
common::Time time;