Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "cartographer/sensor/range_data.h"
00018
00019 #include "cartographer/sensor/proto/sensor.pb.h"
00020 #include "cartographer/transform/transform.h"
00021
00022 namespace cartographer {
00023 namespace sensor {
00024
00025 RangeData TransformRangeData(const RangeData& range_data,
00026 const transform::Rigid3f& transform) {
00027 return RangeData{
00028 transform * range_data.origin,
00029 TransformPointCloud(range_data.returns, transform),
00030 TransformPointCloud(range_data.misses, transform),
00031 };
00032 }
00033
00034 TimedRangeData TransformTimedRangeData(const TimedRangeData& range_data,
00035 const transform::Rigid3f& transform) {
00036 return TimedRangeData{
00037 transform * range_data.origin,
00038 TransformTimedPointCloud(range_data.returns, transform),
00039 TransformTimedPointCloud(range_data.misses, transform),
00040 };
00041 }
00042
00043 RangeData CropRangeData(const RangeData& range_data, const float min_z,
00044 const float max_z) {
00045 return RangeData{range_data.origin,
00046 CropPointCloud(range_data.returns, min_z, max_z),
00047 CropPointCloud(range_data.misses, min_z, max_z)};
00048 }
00049
00050 TimedRangeData CropTimedRangeData(const TimedRangeData& range_data,
00051 const float min_z, const float max_z) {
00052 return TimedRangeData{range_data.origin,
00053 CropTimedPointCloud(range_data.returns, min_z, max_z),
00054 CropTimedPointCloud(range_data.misses, min_z, max_z)};
00055 }
00056
00057 proto::RangeData ToProto(const RangeData& range_data) {
00058 proto::RangeData proto;
00059 *proto.mutable_origin() = transform::ToProto(range_data.origin);
00060 proto.mutable_returns()->Reserve(range_data.returns.size());
00061 for (const RangefinderPoint& point : range_data.returns) {
00062 *proto.add_returns() = ToProto(point);
00063 }
00064 proto.mutable_misses()->Reserve(range_data.misses.size());
00065 for (const RangefinderPoint& point : range_data.misses) {
00066 *proto.add_misses() = ToProto(point);
00067 }
00068 return proto;
00069 }
00070
00071 RangeData FromProto(const proto::RangeData& proto) {
00072 PointCloud returns;
00073 if (proto.returns_size() > 0) {
00074 returns.reserve(proto.returns().size());
00075 for (const auto& point_proto : proto.returns()) {
00076 returns.push_back(FromProto(point_proto));
00077 }
00078 } else {
00079 returns.reserve(proto.returns_legacy().size());
00080 for (const auto& point_proto : proto.returns_legacy()) {
00081 returns.push_back({transform::ToEigen(point_proto)});
00082 }
00083 }
00084 PointCloud misses;
00085 if (proto.misses_size() > 0) {
00086 misses.reserve(proto.misses().size());
00087 for (const auto& point_proto : proto.misses()) {
00088 misses.push_back(FromProto(point_proto));
00089 }
00090 } else {
00091 misses.reserve(proto.misses_legacy().size());
00092 for (const auto& point_proto : proto.misses_legacy()) {
00093 misses.push_back({transform::ToEigen(point_proto)});
00094 }
00095 }
00096 return RangeData{transform::ToEigen(proto.origin()), returns, misses};
00097 }
00098
00099 }
00100 }