源码可在https://github.com/learnmoreonce/SLAM 下载
文件:sensor/range_data.h
#ifndef CARTOGRAPHER_SENSOR_RANGE_DATA_H_
#define CARTOGRAPHER_SENSOR_RANGE_DATA_H_
#include "cartographer/common/port.h"
#include "cartographer/sensor/compressed_point_cloud.h"
#include "cartographer/sensor/point_cloud.h"
#include "cartographer/sensor/proto/sensor.pb.h"
namespace cartographer {
namespace sensor {
/*
RangeData:
数据成员包括
1),原始位置,{x0,y0,z0}
2),返回点云,{x,y,z}
3),缺失点云,标识free space.
*/
// Rays begin at 'origin'. 'returns' are the points where obstructions were
// detected. 'misses' are points in the direction of rays for which no return
// was detected, and were inserted at a configured distance. It is assumed that
// between the 'origin' and 'misses' is free space.
struct RangeData {
Eigen::Vector3f origin;//{x0,y0,z0},sensor坐标。
PointCloud returns; //反射位置{x,y,z},表征有物体反射。
PointCloud misses; //无反射,自由空间
};
// Converts 'range_data' to a proto::RangeData. 序列化
proto::RangeData ToProto(const RangeData& range_data);
// Converts 'proto' to a RangeData. 反序列化
RangeData FromProto(const proto::RangeData& proto);
//对数据进行3d变换,转换为机器坐标
RangeData TransformRangeData(const RangeData& range_data,
const transform::Rigid3f& transform);
//根据min_z和max_z把不在z轴范围内的点云丢弃,剪裁到给定范围
// Crops 'range_data' according to the region defined by 'min_z' and 'max_z'.
RangeData CropRangeData(const RangeData& range_data, float min_z, float max_z);
//压缩后的点云数据
// Like RangeData but with compressed point clouds. The point order changes
// when converting from RangeData.
struct CompressedRangeData {
Eigen::Vector3f origin;
CompressedPointCloud returns;
CompressedPointCloud misses;
};
CompressedRangeData Compress(const RangeData& range_data);
RangeData Decompress(const CompressedRangeData& compressed_range_Data);
} // namespace sensor
} // namespace cartographer
#endif // CARTOGRAPHER_SENSOR_RANGE_DATA_H_
.
文件:sensor/range_data.cc
#include "cartographer/sensor/range_data.h"
#include "cartographer/sensor/proto/sensor.pb.h"
#include "cartographer/transform/transform.h"
namespace cartographer {
namespace sensor {
/*
message PointCloud {
repeated float x = 3 [packed = true];
repeated float y = 4 [packed = true];
repeated float z = 5 [packed = true];
}
message CompressedPointCloud {
optional int32 num_points = 1;
repeated int32 point_data = 3 [packed = true];
}
message RangeData {
optional transform.proto.Vector3f origin = 1;
optional PointCloud point_cloud = 2;
optional PointCloud missing_echo_point_cloud = 3;
}
PointCloud;//vector,元素是3*1f
*/
proto::RangeData ToProto(const RangeData& range_data) {
proto::RangeData proto;
*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);
return proto;
}
RangeData FromProto(const proto::RangeData& proto) {
auto range_data = RangeData{
transform::ToEigen(proto.origin()), ToPointCloud(proto.point_cloud()),
ToPointCloud(proto.missing_echo_point_cloud()),
};
return range_data;
}
/*
将sensor坐标变换为机器坐标。
*/
RangeData TransformRangeData(const RangeData& range_data,
const transform::Rigid3f& transform) {
return RangeData{
transform * range_data.origin,
TransformPointCloud(range_data.returns, transform),
TransformPointCloud(range_data.misses, transform),
};
}
/*
不在给定的z轴范围内的点云删除。
*/
RangeData CropRangeData(const RangeData& range_data, const float min_z,
const float max_z) {
return RangeData{range_data.origin, Crop(range_data.returns, min_z, max_z),
Crop(range_data.misses, min_z, max_z)};
}
/*
压缩,有精度丢失。
*/
CompressedRangeData Compress(const RangeData& range_data) {
return CompressedRangeData{
range_data.origin, CompressedPointCloud(range_data.returns),
CompressedPointCloud(range_data.misses),
};
}
/*
解压缩,有精度丢失
*/
RangeData Decompress(const CompressedRangeData& compressed_range_data) {
return RangeData{compressed_range_data.origin,
compressed_range_data.returns.Decompress(),
compressed_range_data.misses.Decompress()};
}
} // namespace sensor
} // namespace cartographer
测试代码:sensor/range_data_test.h
#include "cartographer/sensor/range_data.h"
#include
#include
#include "gmock/gmock.h"
namespace cartographer {
namespace sensor {
namespace {
using ::testing::Contains;
using ::testing::PrintToString;
// Custom matcher for Eigen::Vector3f entries.
MATCHER_P(ApproximatelyEquals, expected,
string("is equal to ") + PrintToString(expected)) {
return (arg - expected).isZero(0.001f);
}
TEST(RangeDataTest, Compression) {
const std::vector 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), returns, {Eigen::Vector3f(7, 8, 9)}};
//压缩再解压,减少空间占用。
const RangeData actual = Decompress(Compress(range_data));
//isApprox():true if *this is approximately equal to other, within the precision determined by prec.
//压缩前后,精度比较
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_TRUE(actual.misses[0].isApprox(Eigen::Vector3f(7, 8, 9), 0.001f));
// Returns will be reordered, so we compare in an unordered manner.
EXPECT_EQ(3, actual.returns.size());
//是否包含给定值,returns是乱序的。所以使用Contains()函数。
EXPECT_THAT(actual.returns,
Contains(ApproximatelyEquals(Eigen::Vector3f(0, 1, 2))));
EXPECT_THAT(actual.returns,
Contains(ApproximatelyEquals(Eigen::Vector3f(4, 5, 6))));
}
} // namespace
} // namespace sensor
} // namespace cartographer
本文发于:
* http://www.jianshu.com/u/9e38d2febec1
* https://zhuanlan.zhihu.com/learnmoreonce
* http://blog.csdn.net/learnmoreonce
* slam源码分析微信公众号:slamcode