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

文件:sensor/compressed_point_cloud.h

#ifndef CARTOGRAPHER_SENSOR_COMPRESSED_POINT_CLOUD_H_
#define CARTOGRAPHER_SENSOR_COMPRESSED_POINT_CLOUD_H_

#include <iterator>
#include <vector>

#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<int32>& point_data, size_t num_points);

std::vector<int32> 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<int32>::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 <limits>

#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<RasterPoint>>;
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<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) {}

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