cartographer源码分析(18)-sensor-compressed_point_cloud

源码可在https://github.com/learnmoreonce/SLAM 下载

文件:sensor/compressed_point_cloud.h



#ifndef CARTOGRAPHER_SENSOR_COMPRESSED_POINT_CLOUD_H_
#define CARTOGRAPHER_SENSOR_COMPRESSED_POINT_CLOUD_H_

#include 
#include 

#include "Eigen/Core"
#include "cartographer/common/port.h"
#include "cartographer/sensor/point_cloud.h"
#include "cartographer/sensor/proto/sensor.pb.h"

namespace cartographer {
namespace sensor {

/*
CompressedPointCloud是点云压缩类,
目的：压缩ponits以减少存储空间，压缩后有精度损失。
方法：按照block分组。

只有一个私有的
*/
// A compressed representation of a point cloud consisting of a collection of
// points (Vector3f).
// Internally, points are grouped by blocks. Each block encodes a bit of meta
// data (number of points in block, coordinates of the block) and encodes each
// point with a fixed bit rate in relation to the block.
class CompressedPointCloud {
 public:
  class ConstIterator; //前置声明

  CompressedPointCloud() : num_points_(0) {}
  explicit CompressedPointCloud(const PointCloud& point_cloud);

  // Returns decompressed point cloud.
  PointCloud Decompress() const;

  bool empty() const;                   // num_points_==0
  size_t size() const;                  // num_points_
  ConstIterator begin() const;
  ConstIterator end() const;

  proto::CompressedPointCloud ToProto() const;

 private:
  CompressedPointCloud(const std::vector& point_data, size_t num_points);

  std::vector point_data_;
  size_t num_points_;
};

/*前行迭代器*/
// Forward iterator for compressed point clouds.
class CompressedPointCloud::ConstIterator
    : public std::iterator<std::forward_iterator_tag, Eigen::Vector3f> {
 public:
  // Creates begin iterator.
  explicit ConstIterator(const CompressedPointCloud* compressed_point_cloud);

  // Creates end iterator.
  static ConstIterator EndIterator(
      const CompressedPointCloud* compressed_point_cloud);

  Eigen::Vector3f operator*() const;
  ConstIterator& operator++();
  bool operator!=(const ConstIterator& it) const;

 private:
  // Reads next point from buffer. Also handles reading the meta data of the
  // next block, if the current block is depleted.
  void ReadNextPoint();

  const CompressedPointCloud* compressed_point_cloud_;
  size_t remaining_points_;
  int32 remaining_points_in_current_block_;
  Eigen::Vector3f current_point_;
  Eigen::Vector3i current_block_coordinates_;
  std::vector::const_iterator input_;
};

}  // namespace sensor
}  // namespace cartographer

#endif  // CARTOGRAPHER_SENSOR_COMPRESSED_POINT_CLOUD_H_

.

sensor/compressed_point_cloud.cc



#include "cartographer/sensor/compressed_point_cloud.h"

#include 

#include "cartographer/common/math.h"
#include "cartographer/mapping_3d/hybrid_grid.h"

namespace cartographer {
namespace sensor {

namespace {

// Points are encoded on a fixed grid with a grid spacing of 'kPrecision' with
// integers. Points are organized in blocks, where each point is encoded
// relative to the block's origin in an int32 with 'kBitsPerCoordinate' bits per
// coordinate.
constexpr float kPrecision = 0.001f;  // in meters.
constexpr int kBitsPerCoordinate = 10;
constexpr int kCoordinateMask = (1 << kBitsPerCoordinate) - 1;
constexpr int kMaxBitsPerDirection = 23;

}  // namespace

CompressedPointCloud::ConstIterator::ConstIterator(
    const CompressedPointCloud* compressed_point_cloud)
    : compressed_point_cloud_(compressed_point_cloud),
      remaining_points_(compressed_point_cloud->num_points_),
      remaining_points_in_current_block_(0),
      input_(compressed_point_cloud->point_data_.begin()) {
  if (remaining_points_ > 0) {
    ReadNextPoint();
  }
}

CompressedPointCloud::ConstIterator
CompressedPointCloud::ConstIterator::EndIterator(
    const CompressedPointCloud* compressed_point_cloud) {
  ConstIterator end_iterator(compressed_point_cloud);
  end_iterator.remaining_points_ = 0;
  return end_iterator;
}

Eigen::Vector3f CompressedPointCloud::ConstIterator::operator*() const {
  CHECK_GT(remaining_points_, 0);
  return current_point_;
}

CompressedPointCloud::ConstIterator& CompressedPointCloud::ConstIterator::
operator++() {
  --remaining_points_;
  if (remaining_points_ > 0) {
    ReadNextPoint();
  }
  return *this;
}

bool CompressedPointCloud::ConstIterator::operator!=(
    const ConstIterator& it) const {
  CHECK(compressed_point_cloud_ == it.compressed_point_cloud_);
  return remaining_points_ != it.remaining_points_;
}

void CompressedPointCloud::ConstIterator::ReadNextPoint() {
  if (remaining_points_in_current_block_ == 0) {
    remaining_points_in_current_block_ = *input_++;
    for (int i = 0; i < 3; ++i) {
      current_block_coordinates_[i] = *input_++ << kBitsPerCoordinate;
    }
  }
  --remaining_points_in_current_block_;
  const int point = *input_++;
  constexpr int kMask = (1 << kBitsPerCoordinate) - 1;
  current_point_[0] =
      (current_block_coordinates_[0] + (point & kMask)) * kPrecision;
  current_point_[1] = (current_block_coordinates_[1] +
                       ((point >> kBitsPerCoordinate) & kMask)) *
                      kPrecision;
  current_point_[2] =
      (current_block_coordinates_[2] + (point >> (2 * kBitsPerCoordinate))) *
      kPrecision;
}

/*
最重要的构造函数
压缩点云
*/
CompressedPointCloud::CompressedPointCloud(const PointCloud& point_cloud)
//point_cloud是一个3f的vector,压缩到point_data_中储存
    : num_points_(point_cloud.size()) {
  // Distribute points into blocks.
  struct RasterPoint {
    Eigen::Array3i point; //  Array3i (int d1, int d2, int d3)
    int index;
  };
  using Blocks = mapping_3d::HybridGridBase<std::vector>;
  Blocks blocks(kPrecision);
  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];//获取某个point{x,y,z}
    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});
  } //end for

  // 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::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& point_data,
                                           size_t num_points)
    : point_data_(point_data), num_points_(num_points) {}

bool CompressedPointCloud::empty() const { return num_points_ == 0; }

size_t CompressedPointCloud::size() const { return num_points_; }

CompressedPointCloud::ConstIterator CompressedPointCloud::begin() const {//迭代器首,
  return ConstIterator(this);
}

CompressedPointCloud::ConstIterator CompressedPointCloud::end() const { //迭代器尾,
  return ConstIterator::EndIterator(this);
}

PointCloud CompressedPointCloud::Decompress() const {
  PointCloud decompressed;                    //Vector3f组成的vector
  for (const Eigen::Vector3f& point : *this) { //此处调用的是迭代器函数,begin(),end()
    decompressed.push_back(point);
  }
  return decompressed;
}

proto::CompressedPointCloud CompressedPointCloud::ToProto() const {
  proto::CompressedPointCloud result;
  result.set_num_points(num_points_);       //序列化点云的个数
  for (const int32 data : point_data_) {
    result.add_point_data(data);            //依次序添加数据 
  }
  return result;
}

}  // namespace sensor
}  // namespace cartographer

–

测试代码:sensor/compressed_point_cloud_test.cc



#include "cartographer/sensor/compressed_point_cloud.h"

#include "gmock/gmock.h"

namespace Eigen {

// Prints Vector3f in a readable format in matcher ApproximatelyEquals when
// failing a test. Without this function, the output is formated as hexadecimal
// 8 bit numbers.
void PrintTo(const Vector3f& x, std::ostream* os) {
  *os << "(" << x[0] << ", " << x[1] << ", " << x[2] << ")";
}

}  // namespace Eigen

namespace cartographer {
namespace sensor {
namespace {

using ::testing::Contains;
using ::testing::FloatNear;
using ::testing::PrintToString;

constexpr float kPrecision = 0.001f;

// Matcher for 3-d vectors w.r.t. to the target precision.
MATCHER_P(ApproximatelyEquals, expected,
          string("is equal to ") + PrintToString(expected)) {
  return (arg - expected).isZero(kPrecision);//压缩后有精度丢失,精确度为0.001
}

// 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});
  EXPECT_EQ(1, compressed.size());
  EXPECT_THAT(*compressed.begin(), ApproximatelyEquals(p));
  CompressedPointCloud recompressed({*compressed.begin()});
  EXPECT_THAT(*recompressed.begin(), ApproximatelyEquals(p));
}

TEST(CompressPointCloudTest, CompressesPointsCorrectly) {
  TestPoint(Eigen::Vector3f(8000.f, 7500.f, 5000.f));
  TestPoint(Eigen::Vector3f(1000.f, 2000.f, 3000.f));
  TestPoint(Eigen::Vector3f(100.f, 200.f, 300.f));
  TestPoint(Eigen::Vector3f(10.f, 20.f, 30.f));
  TestPoint(Eigen::Vector3f(-0.00049f, -0.0005f, -0.0015f));
  TestPoint(Eigen::Vector3f(0.05119f, 0.0512f, 0.05121));
  TestPoint(Eigen::Vector3f(-0.05119f, -0.0512f, -0.05121));
  TestPoint(Eigen::Vector3f(0.8405f, 0.84f, 0.8396f));
  TestPoint(Eigen::Vector3f(0.8395f, 0.8394f, 0.8393f));
  TestPoint(Eigen::Vector3f(0.839f, 0.8391f, 0.8392f));
  TestPoint(Eigen::Vector3f(0.8389f, 0.8388f, 0.83985f));
}

TEST(CompressPointCloudTest, Compresses) {
  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();
  EXPECT_EQ(3, decompressed.size());
  EXPECT_THAT(decompressed,
              Contains(ApproximatelyEquals(Eigen::Vector3f(0.838f, 0, 0))));//压缩解压缩后,前3位小数不变
  EXPECT_THAT(decompressed,
              Contains(ApproximatelyEquals(Eigen::Vector3f(0.839f, 0, 0))));
  EXPECT_THAT(decompressed,
              Contains(ApproximatelyEquals(Eigen::Vector3f(0.840f, 0, 0))));
}

TEST(CompressPointCloudTest, CompressesEmptyPointCloud) {
  CompressedPointCloud compressed;
  EXPECT_TRUE(compressed.empty());
  EXPECT_EQ(0, compressed.size());
}

// Test for gaps.
// Produces a series of points densly packed along the x axis, compresses these
// points (twice), and tests, whether there are gaps between two consecutive
// points.
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));
  }
  const CompressedPointCloud compressed(point_cloud);//压缩
  const PointCloud decompressed = compressed.Decompress();//解压缩
  const CompressedPointCloud recompressed(decompressed);//再压缩
  EXPECT_EQ(decompressed.size(), recompressed.size());

  std::vector<float> x_coord;
  for (const auto& p : compressed) {
    x_coord.push_back(p[0]);
  }
  std::sort(x_coord.begin(), x_coord.end());
  for (size_t i = 1; i < x_coord.size(); ++i) {
    EXPECT_THAT(std::abs(x_coord[i] - x_coord[i - 1]),
                FloatNear(kPrecision, 1e-7f)); //前后相差不大
  }
}

}  // namespace
}  // namespace sensor
}  // namespace cartographer

本文发于：
* http://www.jianshu.com/u/9e38d2febec1
* https://zhuanlan.zhihu.com/learnmoreonce
* http://blog.csdn.net/learnmoreonce
* slam源码分析微信公众号:slamcode