compressed_point_cloud.cc
Go to the documentation of this file.
00001 /*
00002  * Copyright 2016 The Cartographer Authors
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *      http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 #include "cartographer/sensor/compressed_point_cloud.h"
00018 
00019 #include <limits>
00020 
00021 #include "cartographer/common/math.h"
00022 #include "cartographer/mapping/3d/hybrid_grid.h"
00023 
00024 namespace cartographer {
00025 namespace sensor {
00026 
00027 namespace {
00028 
00029 // Points are encoded on a fixed grid with a grid spacing of 'kPrecision' with
00030 // integers. Points are organized in blocks, where each point is encoded
00031 // relative to the block's origin in an int32 with 'kBitsPerCoordinate' bits per
00032 // coordinate.
00033 constexpr float kPrecision = 0.001f;  // in meters.
00034 constexpr int kBitsPerCoordinate = 10;
00035 constexpr int kCoordinateMask = (1 << kBitsPerCoordinate) - 1;
00036 constexpr int kMaxBitsPerDirection = 23;
00037 
00038 }  // namespace
00039 
00040 CompressedPointCloud::ConstIterator::ConstIterator(
00041     const CompressedPointCloud* compressed_point_cloud)
00042     : compressed_point_cloud_(compressed_point_cloud),
00043       remaining_points_(compressed_point_cloud->num_points_),
00044       remaining_points_in_current_block_(0),
00045       input_(compressed_point_cloud->point_data_.begin()) {
00046   if (remaining_points_ > 0) {
00047     ReadNextPoint();
00048   }
00049 }
00050 
00051 CompressedPointCloud::ConstIterator
00052 CompressedPointCloud::ConstIterator::EndIterator(
00053     const CompressedPointCloud* compressed_point_cloud) {
00054   ConstIterator end_iterator(compressed_point_cloud);
00055   end_iterator.remaining_points_ = 0;
00056   return end_iterator;
00057 }
00058 
00059 RangefinderPoint CompressedPointCloud::ConstIterator::operator*() const {
00060   CHECK_GT(remaining_points_, 0);
00061   return {current_point_};
00062 }
00063 
00064 CompressedPointCloud::ConstIterator&
00065 CompressedPointCloud::ConstIterator::operator++() {
00066   --remaining_points_;
00067   if (remaining_points_ > 0) {
00068     ReadNextPoint();
00069   }
00070   return *this;
00071 }
00072 
00073 bool CompressedPointCloud::ConstIterator::operator!=(
00074     const ConstIterator& it) const {
00075   CHECK(compressed_point_cloud_ == it.compressed_point_cloud_);
00076   return remaining_points_ != it.remaining_points_;
00077 }
00078 
00079 void CompressedPointCloud::ConstIterator::ReadNextPoint() {
00080   if (remaining_points_in_current_block_ == 0) {
00081     remaining_points_in_current_block_ = *input_++;
00082     for (int i = 0; i < 3; ++i) {
00083       current_block_coordinates_[i] = *input_++ << kBitsPerCoordinate;
00084     }
00085   }
00086   --remaining_points_in_current_block_;
00087   const int point = *input_++;
00088   constexpr int kMask = (1 << kBitsPerCoordinate) - 1;
00089   current_point_[0] =
00090       (current_block_coordinates_[0] + (point & kMask)) * kPrecision;
00091   current_point_[1] = (current_block_coordinates_[1] +
00092                        ((point >> kBitsPerCoordinate) & kMask)) *
00093                       kPrecision;
00094   current_point_[2] =
00095       (current_block_coordinates_[2] + (point >> (2 * kBitsPerCoordinate))) *
00096       kPrecision;
00097 }
00098 
00099 CompressedPointCloud::CompressedPointCloud(const PointCloud& point_cloud)
00100     : num_points_(point_cloud.size()) {
00101   // Distribute points into blocks.
00102   struct RasterPoint {
00103     Eigen::Array3i point;
00104     int index;
00105   };
00106   using Blocks = mapping::HybridGridBase<std::vector<RasterPoint>>;
00107   Blocks blocks(kPrecision);
00108   int num_blocks = 0;
00109   CHECK_LE(point_cloud.size(), std::numeric_limits<int>::max());
00110   for (int point_index = 0; point_index < static_cast<int>(point_cloud.size());
00111        ++point_index) {
00112     const RangefinderPoint& point = point_cloud[point_index];
00113     CHECK_LT(point.position.cwiseAbs().maxCoeff() / kPrecision,
00114              1 << kMaxBitsPerDirection)
00115         << "Point out of bounds: " << point.position;
00116     Eigen::Array3i raster_point;
00117     Eigen::Array3i block_coordinate;
00118     for (int i = 0; i < 3; ++i) {
00119       raster_point[i] = common::RoundToInt(point.position[i] / kPrecision);
00120       block_coordinate[i] = raster_point[i] >> kBitsPerCoordinate;
00121       raster_point[i] &= kCoordinateMask;
00122     }
00123     auto* const block = blocks.mutable_value(block_coordinate);
00124     num_blocks += block->empty();
00125     block->push_back({raster_point, point_index});
00126   }
00127 
00128   // Encode blocks.
00129   point_data_.reserve(4 * num_blocks + point_cloud.size());
00130   for (Blocks::Iterator it(blocks); !it.Done(); it.Next(), --num_blocks) {
00131     const auto& raster_points = it.GetValue();
00132     CHECK_LE(raster_points.size(), std::numeric_limits<int32>::max());
00133     point_data_.push_back(raster_points.size());
00134     const Eigen::Array3i block_coordinate = it.GetCellIndex();
00135     point_data_.push_back(block_coordinate.x());
00136     point_data_.push_back(block_coordinate.y());
00137     point_data_.push_back(block_coordinate.z());
00138     for (const RasterPoint& raster_point : raster_points) {
00139       point_data_.push_back((((raster_point.point.z() << kBitsPerCoordinate) +
00140                               raster_point.point.y())
00141                              << kBitsPerCoordinate) +
00142                             raster_point.point.x());
00143     }
00144   }
00145   CHECK_EQ(num_blocks, 0);
00146 }
00147 
00148 CompressedPointCloud::CompressedPointCloud(
00149     const proto::CompressedPointCloud& proto) {
00150   num_points_ = proto.num_points();
00151   const int data_size = proto.point_data_size();
00152   point_data_.reserve(data_size);
00153   // TODO(wohe): Verify that 'point_data_' does not contain malformed data.
00154   for (int i = 0; i != data_size; ++i) {
00155     point_data_.emplace_back(proto.point_data(i));
00156   }
00157 }
00158 
00159 bool CompressedPointCloud::empty() const { return num_points_ == 0; }
00160 
00161 size_t CompressedPointCloud::size() const { return num_points_; }
00162 
00163 CompressedPointCloud::ConstIterator CompressedPointCloud::begin() const {
00164   return ConstIterator(this);
00165 }
00166 
00167 CompressedPointCloud::ConstIterator CompressedPointCloud::end() const {
00168   return ConstIterator::EndIterator(this);
00169 }
00170 
00171 PointCloud CompressedPointCloud::Decompress() const {
00172   PointCloud decompressed;
00173   for (const RangefinderPoint& point : *this) {
00174     decompressed.push_back(point);
00175   }
00176   return decompressed;
00177 }
00178 
00179 bool CompressedPointCloud::operator==(
00180     const CompressedPointCloud& right_hand_container) const {
00181   return point_data_ == right_hand_container.point_data_ &&
00182          num_points_ == right_hand_container.num_points_;
00183 }
00184 
00185 proto::CompressedPointCloud CompressedPointCloud::ToProto() const {
00186   proto::CompressedPointCloud result;
00187   result.set_num_points(num_points_);
00188   for (const int32 data : point_data_) {
00189     result.add_point_data(data);
00190   }
00191   return result;
00192 }
00193 
00194 }  // namespace sensor
00195 }  // namespace cartographer


cartographer
Author(s): The Cartographer Authors
autogenerated on Thu May 9 2019 02:27:35