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

Remove intensities - they are unused in SLAM. (#247)

master
Holger Rapp 2017-05-03 13:44:15 +02:00 committed by GitHub
parent 1cdcd12a8b
commit cdd366bab4
13 changed files with 96 additions and 180 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer/mapping_2d/global_trajectory_builder.cc
View File

@ -37,7 +37,7 @@ void GlobalTrajectoryBuilder::AddRangefinderData(
const sensor::PointCloud& ranges) {
std::unique_ptr<LocalTrajectoryBuilder::InsertionResult> insertion_result =
local_trajectory_builder_.AddHorizontalRangeData(
time, sensor::RangeData{origin, ranges, {}, {}});
time, sensor::RangeData{origin, ranges, {}});
if (insertion_result != nullptr) {
sparse_pose_graph_->AddScan(
insertion_result->time, insertion_result->tracking_to_tracking_2d,

2
cartographer/mapping_2d/local_trajectory_builder.cc
View File

@ -83,7 +83,7 @@ sensor::RangeData LocalTrajectoryBuilder::TransformAndFilterRangeData(
const sensor::RangeData& range_data) const {
// Drop any returns below the minimum range and convert returns beyond the
// maximum range into misses.
sensor::RangeData returns_and_misses{range_data.origin, {}, {}, {}};
sensor::RangeData returns_and_misses{range_data.origin, {}, {}};
for (const Eigen::Vector3f& hit : range_data.returns) {
const float range = (hit - range_data.origin).norm();
if (range >= options_.laser_min_range()) {

2
cartographer/mapping_2d/map_limits_test.cc
View File

@ -69,7 +69,7 @@ TEST(MapLimitsTest, ComputeMapLimits) {
Eigen::Vector3f::Zero(),
{Eigen::Vector3f(-30.f, 1.f, 0.f), Eigen::Vector3f(50.f, -10.f, 0.f)},
{}},
Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}, {}}), nullptr,
Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}), nullptr,
transform::Rigid3d::Identity()};
const mapping::TrajectoryNode trajectory_node{&constant_data,
transform::Rigid3d::Identity()};

27
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -99,7 +99,7 @@ void SparsePoseGraph::AddScan(
constant_node_data_->push_back(mapping::TrajectoryNode::ConstantData{
time, range_data_in_pose,
Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}, {}}), submaps,
Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}), submaps,
tracking_to_pose});
trajectory_nodes_.push_back(mapping::TrajectoryNode{
&constant_node_data_->back(), optimized_pose,
@ -294,20 +294,25 @@ void SparsePoseGraph::WaitForAllComputations() {
common::MutexLocker locker(&mutex_);
const int num_finished_scans_at_start =
constraint_builder_.GetNumFinishedScans();
while (!locker.AwaitWithTimeout([this]() REQUIRES(mutex_) {
while (!locker.AwaitWithTimeout(
[this]() REQUIRES(mutex_) {
return static_cast<size_t>(constraint_builder_.GetNumFinishedScans()) ==
trajectory_nodes_.size();
}, common::FromSeconds(1.))) {
},
common::FromSeconds(1.))) {
std::ostringstream progress_info;
progress_info << "Optimizing: " << std::fixed << std::setprecision(1)
<< 100. * (constraint_builder_.GetNumFinishedScans() -
<< 100. *
(constraint_builder_.GetNumFinishedScans() -
num_finished_scans_at_start) /
(trajectory_nodes_.size() -
num_finished_scans_at_start) << "%...";
num_finished_scans_at_start)
<< "%...";
std::cout << "\r\x1b[K" << progress_info.str() << std::flush;
}
std::cout << "\r\x1b[KOptimizing: Done. " << std::endl;
constraint_builder_.WhenDone([this, &notification](
constraint_builder_.WhenDone(
[this, &notification](
const sparse_pose_graph::ConstraintBuilder::Result& result) {
constraints_.insert(constraints_.end(), result.begin(), result.end());
common::MutexLocker locker(&mutex_);
@ -346,11 +351,11 @@ void SparsePoseGraph::RunOptimization() {
const mapping::Submaps* trajectory =
trajectory_nodes_[i].constant_data->trajectory;
if (extrapolation_transforms.count(trajectory) == 0) {
extrapolation_transforms[trajectory] =
transform::Rigid3d(ExtrapolateSubmapTransforms(submap_transforms_,
*trajectory).back() *
ExtrapolateSubmapTransforms(
optimized_submap_transforms_, *trajectory)
extrapolation_transforms[trajectory] = transform::Rigid3d(
ExtrapolateSubmapTransforms(submap_transforms_, *trajectory)
.back() *
ExtrapolateSubmapTransforms(optimized_submap_transforms_,
*trajectory)
.back()
.inverse());
}

2
cartographer/mapping_3d/kalman_local_trajectory_builder.cc
View File

@ -91,7 +91,7 @@ KalmanLocalTrajectoryBuilder::AddRangefinderData(
const transform::Rigid3f tracking_delta =
first_pose_prediction_.inverse() * pose_prediction.cast<float>();
const sensor::RangeData range_data_in_first_tracking =
sensor::TransformRangeData(sensor::RangeData{origin, ranges, {}, {}},
sensor::TransformRangeData(sensor::RangeData{origin, ranges, {}},
tracking_delta);
for (const Eigen::Vector3f& hit : range_data_in_first_tracking.returns) {
const Eigen::Vector3f delta = hit - range_data_in_first_tracking.origin;

2
cartographer/mapping_3d/submaps.cc
View File

@ -174,7 +174,7 @@ void InsertSegmentsIntoProbabilityGrid(const std::vector<RaySegment>& segments,
// information.
sensor::RangeData FilterRangeDataByMaxRange(const sensor::RangeData& range_data,
const float max_range) {
sensor::RangeData result{range_data.origin, {}, {}, {}};
sensor::RangeData result{range_data.origin, {}, {}};
for (const Eigen::Vector3f& hit : range_data.returns) {
if ((hit - range_data.origin).norm() <= max_range) {
result.returns.push_back(hit);

115
cartographer/sensor/compressed_point_cloud.cc
View File

@ -35,67 +35,6 @@ constexpr int kBitsPerCoordinate = 10;
constexpr int kCoordinateMask = (1 << kBitsPerCoordinate) - 1;
constexpr int kMaxBitsPerDirection = 23;
// Compressed point cloud into a vector and optionally returns mapping from
// compressed indices to original indices.
std::vector<int32> Compress(const PointCloud& point_cloud,
std::vector<int>* new_to_old = nullptr) {
// Distribute points into blocks.
struct RasterPoint {
Eigen::Array3i point;
int index;
};
using Blocks = mapping_3d::HybridGridBase<std::vector<RasterPoint>>;
Blocks blocks(kPrecision, Eigen::Vector3f::Zero());
int num_blocks = 0;
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,
1 << kMaxBitsPerDirection)
<< "Point out of bounds: " << point;
Eigen::Array3i raster_point;
Eigen::Array3i block_coordinate;
for (int i = 0; i < 3; ++i) {
raster_point[i] = common::RoundToInt(point[i] / kPrecision);
block_coordinate[i] = raster_point[i] >> kBitsPerCoordinate;
raster_point[i] &= kCoordinateMask;
}
auto* const block = blocks.mutable_value(block_coordinate);
num_blocks += block->empty();
block->push_back({raster_point, point_index});
}
if (new_to_old != nullptr) {
new_to_old->clear();
new_to_old->reserve(point_cloud.size());
}
// Encode blocks.
std::vector<int32> result;
result.reserve(4 * num_blocks + point_cloud.size());
for (Blocks::Iterator it(blocks); !it.Done(); it.Next(), --num_blocks) {
const auto& raster_points = it.GetValue();
CHECK_LE(raster_points.size(), std::numeric_limits<int32>::max());
result.push_back(raster_points.size());
const Eigen::Array3i block_coordinate = it.GetCellIndex();
result.push_back(block_coordinate.x());
result.push_back(block_coordinate.y());
result.push_back(block_coordinate.z());
for (const RasterPoint& raster_point : raster_points) {
result.push_back((((raster_point.point.z() << kBitsPerCoordinate) +
raster_point.point.y())
<< kBitsPerCoordinate) +
raster_point.point.x());
if (new_to_old != nullptr) {
new_to_old->push_back(raster_point.index);
}
}
}
CHECK_EQ(num_blocks, 0);
return result;
}
} // namespace
CompressedPointCloud::ConstIterator::ConstIterator(
@ -158,18 +97,58 @@ void CompressedPointCloud::ConstIterator::ReadNextPoint() {
}
CompressedPointCloud::CompressedPointCloud(const PointCloud& point_cloud)
: point_data_(Compress(point_cloud)), num_points_(point_cloud.size()) {}
: num_points_(point_cloud.size()) {
// Distribute points into blocks.
struct RasterPoint {
Eigen::Array3i point;
int index;
};
using Blocks = mapping_3d::HybridGridBase<std::vector<RasterPoint>>;
Blocks blocks(kPrecision, Eigen::Vector3f::Zero());
int num_blocks = 0;
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,
1 << kMaxBitsPerDirection)
<< "Point out of bounds: " << point;
Eigen::Array3i raster_point;
Eigen::Array3i block_coordinate;
for (int i = 0; i < 3; ++i) {
raster_point[i] = common::RoundToInt(point[i] / kPrecision);
block_coordinate[i] = raster_point[i] >> kBitsPerCoordinate;
raster_point[i] &= kCoordinateMask;
}
auto* const block = blocks.mutable_value(block_coordinate);
num_blocks += block->empty();
block->push_back({raster_point, point_index});
}
// Encode blocks.
point_data_.reserve(4 * num_blocks + point_cloud.size());
for (Blocks::Iterator it(blocks); !it.Done(); it.Next(), --num_blocks) {
const auto& raster_points = it.GetValue();
CHECK_LE(raster_points.size(), std::numeric_limits<int32>::max());
point_data_.push_back(raster_points.size());
const Eigen::Array3i block_coordinate = it.GetCellIndex();
point_data_.push_back(block_coordinate.x());
point_data_.push_back(block_coordinate.y());
point_data_.push_back(block_coordinate.z());
for (const RasterPoint& raster_point : raster_points) {
point_data_.push_back((((raster_point.point.z() << kBitsPerCoordinate) +
raster_point.point.y())
<< kBitsPerCoordinate) +
raster_point.point.x());
}
}
CHECK_EQ(num_blocks, 0);
}
CompressedPointCloud::CompressedPointCloud(const std::vector<int32>& point_data,
size_t num_points)
: point_data_(point_data), num_points_(num_points) {}
CompressedPointCloud CompressedPointCloud::CompressAndReturnOrder(
const PointCloud& point_cloud, std::vector<int>* new_to_old) {
return CompressedPointCloud(Compress(point_cloud, new_to_old),
point_cloud.size());
}
bool CompressedPointCloud::empty() const { return num_points_ == 0; }
size_t CompressedPointCloud::size() const { return num_points_; }

10
cartographer/sensor/compressed_point_cloud.h
View File

@ -40,12 +40,6 @@ class CompressedPointCloud {
CompressedPointCloud() : num_points_(0) {}
explicit CompressedPointCloud(const PointCloud& point_cloud);
// Returns a compressed point cloud and further returns a mapping 'new_to_old'
// from the compressed indices to the original indices, i.e., conceptually
// compressed[i] = point_cloud[new_to_old[i]].
static CompressedPointCloud CompressAndReturnOrder(
const PointCloud& point_cloud, std::vector<int>* new_to_old);
// Returns decompressed point cloud.
PointCloud Decompress() const;
@ -59,8 +53,8 @@ class CompressedPointCloud {
private:
CompressedPointCloud(const std::vector<int32>& point_data, size_t num_points);
const std::vector<int32> point_data_;
const size_t num_points_;
std::vector<int32> point_data_;
size_t num_points_;
};
// Forward iterator for compressed point clouds.

15
cartographer/sensor/compressed_point_cloud_test.cc
View File

@ -116,21 +116,6 @@ TEST(CompressPointCloudTest, CompressesNoGaps) {
}
}
TEST(CompressPointCloudTest, CompressesAndReturnsOrder) {
PointCloud point_cloud = {
Eigen::Vector3f(1.f, 0.f, 0.f), Eigen::Vector3f(-10.f, 0.f, 0.f),
Eigen::Vector3f(1.f, 0.f, 0.f), Eigen::Vector3f(-10.f, 0.f, 0.f)};
std::vector<int> new_to_old;
CompressedPointCloud compressed =
CompressedPointCloud::CompressAndReturnOrder(point_cloud, &new_to_old);
EXPECT_EQ(point_cloud.size(), new_to_old.size());
int i = 0;
for (const Eigen::Vector3f& point : compressed) {
EXPECT_THAT(point, ApproximatelyEquals(point_cloud[new_to_old[i]]));
++i;
}
}
} // namespace
} // namespace sensor
} // namespace cartographer

3
cartographer/sensor/proto/sensor.proto
View File

@ -74,7 +74,4 @@ message RangeData {
optional transform.proto.Vector3f origin = 1;
optional PointCloud point_cloud = 2;
optional PointCloud missing_echo_point_cloud = 3;
// Reflectivity values of point_cloud or empty.
repeated int32 reflectivity = 4 [packed = true];
}

31
cartographer/sensor/range_data.cc
View File

@ -22,21 +22,6 @@
namespace cartographer {
namespace sensor {
namespace {
// Reorders reflectivities according to index mapping.
std::vector<uint8> ReorderReflectivities(
const std::vector<uint8>& reflectivities,
const std::vector<int>& new_to_old) {
std::vector<uint8> reordered(reflectivities.size());
for (size_t i = 0; i < reordered.size(); ++i) {
reordered[i] = reflectivities[new_to_old[i]];
}
return reordered;
}
} // namespace
PointCloud ToPointCloud(const proto::LaserScan& proto) {
return ToPointCloudWithIntensities(proto).points;
}
@ -77,8 +62,6 @@ proto::RangeData ToProto(const RangeData& range_data) {
*proto.mutable_origin() = transform::ToProto(range_data.origin);
*proto.mutable_point_cloud() = ToProto(range_data.returns);
*proto.mutable_missing_echo_point_cloud() = ToProto(range_data.misses);
std::copy(range_data.reflectivities.begin(), range_data.reflectivities.end(),
RepeatedFieldBackInserter(proto.mutable_reflectivity()));
return proto;
}
@ -87,8 +70,6 @@ RangeData FromProto(const proto::RangeData& proto) {
transform::ToEigen(proto.origin()), ToPointCloud(proto.point_cloud()),
ToPointCloud(proto.missing_echo_point_cloud()),
};
std::copy(proto.reflectivity().begin(), proto.reflectivity().end(),
std::back_inserter(range_data.reflectivities));
return range_data;
}
@ -98,7 +79,6 @@ RangeData TransformRangeData(const RangeData& range_data,
transform * range_data.origin,
TransformPointCloud(range_data.returns, transform),
TransformPointCloud(range_data.misses, transform),
range_data.reflectivities,
};
}
@ -109,21 +89,16 @@ RangeData CropRangeData(const RangeData& range_data, const float min_z,
}
CompressedRangeData Compress(const RangeData& range_data) {
std::vector<int> new_to_old;
CompressedPointCloud compressed_returns =
CompressedPointCloud::CompressAndReturnOrder(range_data.returns,
&new_to_old);
return CompressedRangeData{
range_data.origin, std::move(compressed_returns),
range_data.origin, CompressedPointCloud(range_data.returns),
CompressedPointCloud(range_data.misses),
ReorderReflectivities(range_data.reflectivities, new_to_old)};
};
}
RangeData Decompress(const CompressedRangeData& compressed_range_data) {
return RangeData{compressed_range_data.origin,
compressed_range_data.returns.Decompress(),
compressed_range_data.misses.Decompress(),
compressed_range_data.reflectivities};
compressed_range_data.misses.Decompress()};
}
} // namespace sensor

6
cartographer/sensor/range_data.h
View File

@ -33,9 +33,6 @@ struct RangeData {
Eigen::Vector3f origin;
PointCloud returns;
PointCloud misses;
// Reflectivity value of returns.
std::vector<uint8> reflectivities;
};
// Builds a PointCloud of returns from 'proto', dropping any beams with ranges
@ -67,9 +64,6 @@ struct CompressedRangeData {
Eigen::Vector3f origin;
CompressedPointCloud returns;
CompressedPointCloud misses;
// Reflectivity value of returns.
std::vector<uint8> reflectivities;
};
CompressedRangeData Compress(const RangeData& range_data);

39
cartographer/sensor/range_data_test.cc
View File

@ -74,43 +74,30 @@ TEST(LaserTest, ToPointCloudWithInfinityAndNaN) {
EXPECT_TRUE(point_cloud[1].isApprox(Eigen::Vector3f(-3.f, 0.f, 0.f), 1e-6));
}
// Custom matcher for pair<eigen::Vector3f, int> entries.
MATCHER_P(PairApproximatelyEquals, expected,
// Custom matcher for Eigen::Vector3f entries.
MATCHER_P(ApproximatelyEquals, expected,
string("is equal to ") + PrintToString(expected)) {
return (arg.first - expected.first).isZero(0.001f) &&
arg.second == expected.second;
return (arg - expected).isZero(0.001f);
}
TEST(RangeDataTest, Compression) {
const std::vector<Eigen::Vector3f> returns = {Eigen::Vector3f(0, 1, 2),
Eigen::Vector3f(4, 5, 6),
Eigen::Vector3f(0, 1, 2)};
const RangeData range_data = {
Eigen::Vector3f(1, 1, 1),
{Eigen::Vector3f(0, 1, 2), Eigen::Vector3f(4, 5, 6),
Eigen::Vector3f(0, 1, 2)},
{Eigen::Vector3f(7, 8, 9)},
{1, 2, 3}};
Eigen::Vector3f(1, 1, 1), returns, {Eigen::Vector3f(7, 8, 9)}};
const RangeData actual = Decompress(Compress(range_data));
EXPECT_TRUE(actual.origin.isApprox(Eigen::Vector3f(1, 1, 1), 1e-6));
EXPECT_EQ(3, actual.returns.size());
EXPECT_EQ(1, actual.misses.size());
EXPECT_EQ(actual.returns.size(), actual.reflectivities.size());
EXPECT_TRUE(actual.misses[0].isApprox(Eigen::Vector3f(7, 8, 9), 0.001f));
// Returns and their corresponding reflectivities will be reordered, so we
// pair them up into a vector, and compare in an unordered manner.
std::vector<std::pair<Eigen::Vector3f, int>> pairs;
for (size_t i = 0; i < actual.returns.size(); ++i) {
pairs.emplace_back(actual.returns[i], actual.reflectivities[i]);
}
EXPECT_EQ(3, pairs.size());
EXPECT_THAT(pairs,
Contains(PairApproximatelyEquals(std::pair<Eigen::Vector3f, int>(
Eigen::Vector3f(0, 1, 2), 1))));
EXPECT_THAT(pairs,
Contains(PairApproximatelyEquals(std::pair<Eigen::Vector3f, int>(
Eigen::Vector3f(0, 1, 2), 3))));
EXPECT_THAT(pairs,
Contains(PairApproximatelyEquals(std::pair<Eigen::Vector3f, int>(
Eigen::Vector3f(4, 5, 6), 2))));
// Returns will be reordered, so we compare in an unordered manner.
EXPECT_EQ(3, actual.returns.size());
EXPECT_THAT(actual.returns,
Contains(ApproximatelyEquals(Eigen::Vector3f(0, 1, 2))));
EXPECT_THAT(actual.returns,
Contains(ApproximatelyEquals(Eigen::Vector3f(4, 5, 6))));
}
} // namespace