compressed_point_cloud.cc
Go to the documentation of this file.
1 /*
2  * Copyright 2016 The Cartographer Authors
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
18 
19 #include <limits>
20 
23 
24 namespace cartographer {
25 namespace sensor {
26 
27 namespace {
28 
29 // Points are encoded on a fixed grid with a grid spacing of 'kPrecision' with
30 // integers. Points are organized in blocks, where each point is encoded
31 // relative to the block's origin in an int32 with 'kBitsPerCoordinate' bits per
32 // coordinate.
33 constexpr float kPrecision = 0.001f; // in meters.
34 constexpr int kBitsPerCoordinate = 10;
35 constexpr int kCoordinateMask = (1 << kBitsPerCoordinate) - 1;
36 constexpr int kMaxBitsPerDirection = 23;
37 
38 } // namespace
39 
41  const CompressedPointCloud* compressed_point_cloud)
42  : compressed_point_cloud_(compressed_point_cloud),
43  remaining_points_(compressed_point_cloud->num_points_),
44  remaining_points_in_current_block_(0),
45  input_(compressed_point_cloud->point_data_.begin()) {
46  if (remaining_points_ > 0) {
47  ReadNextPoint();
48  }
49 }
50 
53  const CompressedPointCloud* compressed_point_cloud) {
54  ConstIterator end_iterator(compressed_point_cloud);
55  end_iterator.remaining_points_ = 0;
56  return end_iterator;
57 }
58 
60  CHECK_GT(remaining_points_, 0);
61  return current_point_;
62 }
63 
67  if (remaining_points_ > 0) {
68  ReadNextPoint();
69  }
70  return *this;
71 }
72 
74  const ConstIterator& it) const {
77 }
78 
82  for (int i = 0; i < 3; ++i) {
83  current_block_coordinates_[i] = *input_++ << kBitsPerCoordinate;
84  }
85  }
87  const int point = *input_++;
88  constexpr int kMask = (1 << kBitsPerCoordinate) - 1;
89  current_point_[0] =
90  (current_block_coordinates_[0] + (point & kMask)) * kPrecision;
92  ((point >> kBitsPerCoordinate) & kMask)) *
93  kPrecision;
94  current_point_[2] =
95  (current_block_coordinates_[2] + (point >> (2 * kBitsPerCoordinate))) *
96  kPrecision;
97 }
98 
100  : num_points_(point_cloud.size()) {
101  // Distribute points into blocks.
102  struct RasterPoint {
103  Eigen::Array3i point;
104  int index;
105  };
107  Blocks blocks(kPrecision);
108  int num_blocks = 0;
109  CHECK_LE(point_cloud.size(), std::numeric_limits<int>::max());
110  for (int point_index = 0; point_index < static_cast<int>(point_cloud.size());
111  ++point_index) {
112  const Eigen::Vector3f& point = point_cloud[point_index];
113  CHECK_LT(point.cwiseAbs().maxCoeff() / kPrecision,
114  1 << kMaxBitsPerDirection)
115  << "Point out of bounds: " << point;
116  Eigen::Array3i raster_point;
117  Eigen::Array3i block_coordinate;
118  for (int i = 0; i < 3; ++i) {
119  raster_point[i] = common::RoundToInt(point[i] / kPrecision);
120  block_coordinate[i] = raster_point[i] >> kBitsPerCoordinate;
121  raster_point[i] &= kCoordinateMask;
122  }
123  auto* const block = blocks.mutable_value(block_coordinate);
124  num_blocks += block->empty();
125  block->push_back({raster_point, point_index});
126  }
127 
128  // Encode blocks.
129  point_data_.reserve(4 * num_blocks + point_cloud.size());
130  for (Blocks::Iterator it(blocks); !it.Done(); it.Next(), --num_blocks) {
131  const auto& raster_points = it.GetValue();
132  CHECK_LE(raster_points.size(), std::numeric_limits<int32>::max());
133  point_data_.push_back(raster_points.size());
134  const Eigen::Array3i block_coordinate = it.GetCellIndex();
135  point_data_.push_back(block_coordinate.x());
136  point_data_.push_back(block_coordinate.y());
137  point_data_.push_back(block_coordinate.z());
138  for (const RasterPoint& raster_point : raster_points) {
139  point_data_.push_back((((raster_point.point.z() << kBitsPerCoordinate) +
140  raster_point.point.y())
141  << kBitsPerCoordinate) +
142  raster_point.point.x());
143  }
144  }
145  CHECK_EQ(num_blocks, 0);
146 }
147 
148 CompressedPointCloud::CompressedPointCloud(const std::vector<int32>& point_data,
149  size_t num_points)
150  : point_data_(point_data), num_points_(num_points) {}
151 
152 bool CompressedPointCloud::empty() const { return num_points_ == 0; }
153 
154 size_t CompressedPointCloud::size() const { return num_points_; }
155 
157  return ConstIterator(this);
158 }
159 
161  return ConstIterator::EndIterator(this);
162 }
163 
165  PointCloud decompressed;
166  for (const Eigen::Vector3f& point : *this) {
167  decompressed.push_back(point);
168  }
169  return decompressed;
170 }
171 
172 proto::CompressedPointCloud CompressedPointCloud::ToProto() const {
173  proto::CompressedPointCloud result;
174  result.set_num_points(num_points_);
175  for (const int32 data : point_data_) {
176  result.add_point_data(data);
177  }
178  return result;
179 }
180 
181 } // namespace sensor
182 } // namespace cartographer
int RoundToInt(const float x)
Definition: port.h:42
ConstIterator(const CompressedPointCloud *compressed_point_cloud)
int32_t int32
Definition: port.h:30
static ConstIterator EndIterator(const CompressedPointCloud *compressed_point_cloud)
proto::CompressedPointCloud ToProto() const
std::vector< Eigen::Vector3f > PointCloud
Definition: point_cloud.h:30


cartographer
Author(s):
autogenerated on Wed Jun 5 2019 21:57:58