172 #include <string_view>
173 #include <type_traits>
174 #include <unordered_map>
177 #include "../base/bytes.h"
178 #include "../base/quantize.h"
200 enum Type : uint8_t {
228 using KeyMap = std::unordered_map<std::string, uint16_t>;
233 using KeyList = std::vector<Key>;
256 explicit Vector3f(
float _x,
float _y,
float _z) noexcept;
289 explicit Vector3d(
double _x,
double _y,
double _z) noexcept;
347 bool operator<<(const Bytes& bytes) noexcept;
357 bool operator>>(Bytes& bytes) const noexcept;
365 [[nodiscard]] static
bool check_valid(const Bytes& bytes) noexcept;
372 [[nodiscard]]
bool is_valid() const noexcept;
380 bool shallow_copy(const
PointCloud& target) noexcept;
388 bool deep_copy(const
PointCloud& target) noexcept;
412 [[nodiscard]] KeyMap get_key_map(KeyList* key_list =
nullptr) const noexcept;
419 [[nodiscard]] KeyList get_key_list() const noexcept;
426 [[nodiscard]]
size_t size() const noexcept;
433 [[nodiscard]]
size_t pack_size() const noexcept;
440 [[nodiscard]]
bool is_owner() const noexcept;
447 [[nodiscard]] uint16_t get_extent() const noexcept;
454 [[nodiscard]]
bool get_vertical() const noexcept;
466 void set_vertical(
bool vertical) noexcept;
473 [[nodiscard]] uint8_t get_downsample() const noexcept;
487 bool downsample(uint8_t level) noexcept;
494 [[nodiscard]] uint64_t get_protocol_size_num() const noexcept;
501 [[nodiscard]] uint64_t get_protocol_type_num() const noexcept;
508 [[nodiscard]] std::
string get_protocol_size_str() const noexcept;
516 [[nodiscard]] std::
string get_protocol_name_str() const noexcept;
523 [[nodiscard]] std::
string get_protocol_type_str() const noexcept;
530 [[nodiscard]] const uint8_t* get_internal_data() const noexcept;
537 [[nodiscard]]
size_t get_reserved_size() const noexcept;
548 bool get_value_v3f(
float& x,
float& y,
float& z,
size_t loop_index) const noexcept;
557 bool get_value_v3f(
Vector3f& v3f,
size_t loop_index) const noexcept;
565 [[nodiscard]]
Vector3f get_value_v3f(
size_t loop_index) const noexcept;
576 bool get_value_v3d(
double& x,
double& y,
double& z,
size_t loop_index) const noexcept;
585 bool get_value_v3d(
Vector3d& v3d,
size_t loop_index) const noexcept;
593 [[nodiscard]]
Vector3d get_value_v3d(
size_t loop_index) const noexcept;
604 template <typename T>
605 bool get_value(T& t,
size_t loop_index, uint16_t offset) const noexcept;
615 template <typename T>
616 [[nodiscard]] T get_value(
size_t loop_index, uint16_t offset) const noexcept;
628 template <typename T>
629 bool get_value(T& t,
size_t loop_index, KeyMap& key_map, std::string_view key) const noexcept;
640 template <typename T>
641 [[nodiscard]] T get_value(
size_t loop_index, KeyMap& key_map, std::string_view key) const noexcept;
651 [[nodiscard]]
double get_value_for_double_float(
size_t loop_index, uint16_t offset, uint8_t type) const noexcept;
662 [[nodiscard]]
double get_value_for_double_float(
size_t loop_index, KeyMap& key_map, std::string_view key,
663 uint8_t type) const noexcept;
673 [[nodiscard]] std::
string get_value_for_print(
size_t loop_index, uint16_t offset, uint8_t type) const noexcept;
684 [[nodiscard]] std::
string get_value_for_print(
size_t loop_index, KeyMap& key_map, std::string_view key,
685 uint8_t type) const noexcept;
704 bool create(
size_t size, uint64_t size_num, uint64_t type_num, std::string_view key_str, uint16_t extent = 0,
705 bool vertical = false) noexcept;
717 template <typename... T>
718 bool create(
size_t _size, const std::vector<std::
string>& keys = {}, uint16_t extent = 0,
719 bool vertical =
false) noexcept;
731 template <
typename... T>
732 bool create_v3f(
size_t _size,
const std::vector<std::string>& keys = {}, uint16_t extent = 0,
733 bool vertical =
false) noexcept;
745 template <
typename... T>
746 bool create_v3d(
size_t _size,
const std::vector<std::string>& keys = {}, uint16_t extent = 0,
747 bool vertical =
false) noexcept;
756 bool fill_packed_data(
const uint8_t* src_data,
size_t _size) noexcept;
765 template <
typename... T>
766 bool push_value(T... args) noexcept;
779 template <
typename... T>
780 bool push_value_v3f(
float x,
float y,
float z, T... args) noexcept;
791 template <
typename... T>
792 bool push_value_v3f(
Vector3f v3f, T... args) noexcept;
805 template <
typename... T>
806 bool push_value_v3d(
double x,
double y,
double z, T... args) noexcept;
817 template <
typename... T>
818 bool push_value_v3d(
Vector3d v3d, T... args) noexcept;
830 bool resize(
size_t size) noexcept;
844 template <
typename... T>
845 bool set_value(
size_t loop_index, T... args) noexcept;
859 template <
typename... T>
860 bool set_value_v3f(
size_t loop_index,
float x,
float y,
float z, T... args) noexcept;
872 template <
typename... T>
873 bool set_value_v3f(
size_t loop_index,
Vector3f v3f, T... args) noexcept;
887 template <
typename... T>
888 bool set_value_v3d(
size_t loop_index,
double x,
double y,
double z, T... args) noexcept;
900 template <
typename... T>
901 bool set_value_v3d(
size_t loop_index,
Vector3d v3d, T... args) noexcept;
914 void clear(
bool force =
false) noexcept;
926 uint32_t& get_reserved() noexcept {
return reserved_buf_; }
938 uint32_t& get_reserved2() noexcept {
return reserved_buf2_; }
950 uint8_t& get_reserved3() noexcept {
return reserved_buf3_; }
954 static constexpr
bool kZerocopyTypes{
true};
958 uint64_t size_num{0};
959 uint64_t type_num{0};
962 Protocol() noexcept = default;
964 template <typename... T>
965 static uint64_t get_size_num() noexcept;
967 template <typename... T>
968 static uint64_t get_type_num() noexcept;
970 template <typename T>
971 static constexpr uint8_t get_type() noexcept;
973 constexpr uint64_t get_pack_size() const noexcept;
975 static
bool check_valid(uint64_t _size_num, std::string_view _names) noexcept;
977 static std::
string get_names(const std::vector<std::
string>& keys) noexcept;
979 KeyList get_key_list() const noexcept;
981 std::
string get_size_for_print() const noexcept;
983 std::
string get_type_for_print() const noexcept;
986 template <typename T>
987 static
bool xyz_in_extent(T x, T y, T z, uint16_t extent) noexcept;
989 bool compress_protocol_xyz() noexcept;
992 uint8_t* data_{
nullptr};
995 uint32_t reserved_buf_{0};
996 uint32_t reserved_buf2_{0};
997 uint16_t pack_size_{0};
999 uint8_t downsample_{0};
1000 uint8_t reserved_buf3_{0};
1001 bool vertical_{
false};
1002 bool is_owner_{
false};
1005 static constexpr uint32_t kMagicNumberBegin{0x98B7F16A};
1006 static constexpr uint32_t kMagicNumberEnd{0x98B7F16F};
1013 template <
typename T>
1014 inline bool PointCloud::get_value(T& t,
size_t loop_index, uint16_t offset)
const noexcept {
1015 static_assert(std::is_fundamental_v<T>,
"T must be fundamental.");
1017 if VUNLIKELY (!data_ || loop_index >= size_ || offset > pack_size_) {
1018 std::memset(&t, 0,
sizeof(T));
1022 if (extent_ != 0 && offset <
sizeof(int16_t) * 3) {
1023 if constexpr (std::is_same_v<T, float> || std::is_same_v<T, double>) {
1026 if VUNLIKELY (
sizeof(value) >
static_cast<size_t>(pack_size_ - offset)) {
1031 const size_t p = (loop_index * pack_size_) + offset;
1032 std::memcpy(&value, data_ + p,
sizeof(value));
1033 t = Quantize::decode<T>(extent_, value);
1039 if VUNLIKELY (
sizeof(T) >
static_cast<size_t>(pack_size_ - offset)) {
1040 std::memset(&t, 0,
sizeof(T));
1044 const size_t p = (loop_index * pack_size_) + offset;
1045 std::memcpy(&t, data_ + p,
sizeof(T));
1050 template <
typename T>
1051 inline T PointCloud::get_value(
size_t loop_index, uint16_t offset)
const noexcept {
1052 static_assert(std::is_fundamental_v<T>,
"T must be fundamental.");
1056 get_value(t, loop_index, offset);
1061 template <
typename T>
1062 inline bool PointCloud::get_value(T& t,
size_t loop_index, KeyMap& key_map, std::string_view key)
const noexcept {
1063 static_assert(std::is_fundamental_v<T>,
"T must be fundamental.");
1065 const std::string lookup(key);
1066 auto iter = key_map.find(lookup);
1069 std::memset(&t, 0,
sizeof(T));
1073 return get_value<T>(t, loop_index, iter->second);
1076 template <
typename T>
1077 inline T PointCloud::get_value(
size_t loop_index, KeyMap& key_map, std::string_view key)
const noexcept {
1078 static_assert(std::is_fundamental_v<T>,
"T must be fundamental.");
1082 get_value(t, loop_index, key_map, key);
1087 template <
typename... T>
1088 inline bool PointCloud::create(
size_t size,
const std::vector<std::string>& keys, uint16_t extent,
1089 bool vertical) noexcept {
1090 static_assert((std::is_fundamental_v<T> && ...),
"All types must be fundamental.");
1092 static_assert(
sizeof...(T) >= 3 &&
sizeof...(T) <= 16,
"The number of keys ranges is [3 ~ 16].");
1094 static_assert(((Protocol::get_type<T>() !=
kUnknownType) && ...),
1095 "All field types must map to a supported PointCloud::Type.");
1097 if VUNLIKELY (
sizeof...(T) != keys.size()) {
1101 uint64_t size_num = Protocol::get_size_num<T...>();
1107 std::string key_str = Protocol::get_names(keys);
1113 uint64_t type_num = Protocol::get_type_num<T...>();
1115 Protocol new_protocol{};
1116 new_protocol.size_num = size_num;
1117 std::memset(new_protocol.names, 0,
sizeof(new_protocol.names));
1118 std::memcpy(new_protocol.names, key_str.c_str(), key_str.size());
1119 new_protocol.type_num = type_num;
1123 protocol_probe.protocol_ = new_protocol;
1125 if VUNLIKELY (!protocol_probe.compress_protocol_xyz()) {
1129 new_protocol = protocol_probe.protocol_;
1132 const size_t new_pack_size = new_protocol.get_pack_size();
1134 if VUNLIKELY (new_pack_size != 0 && size > std::numeric_limits<size_t>::max() / new_pack_size) {
1138 if (is_owner_ && data_ && capacity_ != 0) {
1148 protocol_ = new_protocol;
1150 vertical_ = vertical;
1153 pack_size_ = new_pack_size;
1154 capacity_ = size * pack_size_;
1170 template <
typename... T>
1171 inline bool PointCloud::create_v3f(
size_t _size,
const std::vector<std::string>& keys, uint16_t extent,
1172 bool vertical) noexcept {
1173 std::vector<std::string> target_keys{
"x",
"y",
"z"};
1175 target_keys.insert(target_keys.end(), keys.begin(), keys.end());
1177 return create<float, float, float, T...>(_size, target_keys, extent, vertical);
1180 template <
typename... T>
1181 inline bool PointCloud::create_v3d(
size_t _size,
const std::vector<std::string>& keys, uint16_t extent,
1182 bool vertical) noexcept {
1183 std::vector<std::string> target_keys{
"x",
"y",
"z"};
1185 target_keys.insert(target_keys.end(), keys.begin(), keys.end());
1187 return create<double, double, double, T...>(_size, target_keys, extent, vertical);
1190 inline bool PointCloud::fill_packed_data(
const uint8_t* src_data,
size_t _size) noexcept {
1191 bool is_fill_success =
false;
1193 if VUNLIKELY (_size == 0 || !src_data) {
1194 is_fill_success =
false;
1195 }
else if VUNLIKELY (!is_owner_ || !data_ || pack_size_ == 0 || capacity_ == 0) {
1196 is_fill_success =
false;
1197 }
else if VUNLIKELY (_size > std::numeric_limits<size_t>::max() / pack_size_ || _size * pack_size_ > capacity_) {
1198 is_fill_success =
false;
1200 std::memcpy(data_, src_data, _size * pack_size_);
1202 index_ = _size * pack_size_;
1203 is_fill_success =
true;
1206 return is_fill_success;
1209 template <
typename... T>
1210 inline bool PointCloud::push_value(T... args) noexcept {
1211 static_assert((std::is_fundamental_v<T> && ...),
"All types must be fundamental.");
1213 static_assert(
sizeof...(T) >= 3 &&
sizeof...(T) <= 16,
"The number of keys ranges is [3 ~ 16].");
1215 if VUNLIKELY (!is_owner_ || !data_ || pack_size_ == 0 || capacity_ == 0) {
1219 if VUNLIKELY (size_ * pack_size_ >= capacity_) {
1223 constexpr
size_t kTargetPackSize = (
sizeof(T) + ...);
1225 if VUNLIKELY (kTargetPackSize != pack_size_) {
1229 auto* target_ptr = data_ + index_;
1233 std::memcpy(target_ptr, &args,
sizeof(args));
1234 target_ptr +=
sizeof(args);
1238 index_ += pack_size_;
1244 template <
typename... T>
1245 inline bool PointCloud::push_value_v3f(
float x,
float y,
float z, T... args) noexcept {
1247 if VUNLIKELY (!xyz_in_extent(x, y, z, extent_)) {
1251 auto qx = Quantize::encode<int16_t>(extent_, x);
1252 auto qy = Quantize::encode<int16_t>(extent_, y);
1253 auto qz = Quantize::encode<int16_t>(extent_, z);
1255 return push_value(qx, qy, qz, args...);
1258 return push_value(x, y, z, args...);
1261 template <
typename... T>
1262 inline bool PointCloud::push_value_v3f(
Vector3f v3f, T... args) noexcept {
1263 return push_value_v3f(v3f.x, v3f.y, v3f.z, args...);
1266 template <
typename... T>
1267 inline bool PointCloud::push_value_v3d(
double x,
double y,
double z, T... args) noexcept {
1269 if VUNLIKELY (!xyz_in_extent(x, y, z, extent_)) {
1273 auto qx = Quantize::encode<int16_t>(extent_, x);
1274 auto qy = Quantize::encode<int16_t>(extent_, y);
1275 auto qz = Quantize::encode<int16_t>(extent_, z);
1277 return push_value(qx, qy, qz, args...);
1280 return push_value(x, y, z, args...);
1283 template <
typename... T>
1284 inline bool PointCloud::push_value_v3d(
Vector3d v3d, T... args) noexcept {
1285 return push_value_v3d(v3d.x, v3d.y, v3d.z, args...);
1288 inline bool PointCloud::resize(
size_t size) noexcept {
1289 if VUNLIKELY (!is_owner_ || !data_ || pack_size_ == 0 || capacity_ == 0) {
1293 if VUNLIKELY (size > std::numeric_limits<size_t>::max() / pack_size_) {
1297 if VUNLIKELY (size * pack_size_ > capacity_) {
1302 index_ = size_ * pack_size_;
1307 template <
typename... T>
1308 bool PointCloud::set_value(
size_t loop_index, T... args) noexcept {
1309 static_assert((std::is_fundamental_v<T> && ...),
"All types must be fundamental.");
1311 static_assert(
sizeof...(T) >= 3 &&
sizeof...(T) <= 16,
"The number of keys ranges is [3 ~ 16].");
1313 if VUNLIKELY (!is_owner_ || !data_ || pack_size_ == 0 || capacity_ == 0) {
1317 if VUNLIKELY (loop_index >= size_ || size_ * pack_size_ > capacity_) {
1321 if VUNLIKELY (index_ != size_ * pack_size_) {
1322 std::cerr <<
"[PointCloud::set_value] Invalid buffer state: "
1323 <<
"The current buffer size does not match the capacity. "
1324 <<
"Please call resize(size) before using set_value()." << std::endl;
1328 constexpr
size_t kTargetPackSize = (
sizeof(T) + ...);
1330 if VUNLIKELY (kTargetPackSize != pack_size_) {
1334 auto* target_ptr = data_ + (loop_index * pack_size_);
1338 std::memcpy(target_ptr, &args,
sizeof(args));
1339 target_ptr +=
sizeof(args);
1346 template <
typename... T>
1347 inline bool PointCloud::set_value_v3f(
size_t loop_index,
float x,
float y,
float z, T... args) noexcept {
1349 if VUNLIKELY (!xyz_in_extent(x, y, z, extent_)) {
1353 auto qx = Quantize::encode<int16_t>(extent_, x);
1354 auto qy = Quantize::encode<int16_t>(extent_, y);
1355 auto qz = Quantize::encode<int16_t>(extent_, z);
1357 return set_value(loop_index, qx, qy, qz, args...);
1360 return set_value(loop_index, x, y, z, args...);
1363 template <
typename... T>
1364 inline bool PointCloud::set_value_v3f(
size_t loop_index,
Vector3f v3f, T... args) noexcept {
1365 return set_value_v3f(loop_index, v3f.x, v3f.y, v3f.z, args...);
1368 template <
typename... T>
1369 inline bool PointCloud::set_value_v3d(
size_t loop_index,
double x,
double y,
double z, T... args) noexcept {
1371 if VUNLIKELY (!xyz_in_extent(x, y, z, extent_)) {
1375 auto qx = Quantize::encode<int16_t>(extent_, x);
1376 auto qy = Quantize::encode<int16_t>(extent_, y);
1377 auto qz = Quantize::encode<int16_t>(extent_, z);
1379 return set_value(loop_index, qx, qy, qz, args...);
1382 return set_value(loop_index, x, y, z, args...);
1385 template <
typename... T>
1386 inline bool PointCloud::set_value_v3d(
size_t loop_index,
Vector3d v3d, T... args) noexcept {
1387 return set_value_v3d(loop_index, v3d.x, v3d.y, v3d.z, args...);
1390 template <
typename... T>
1391 inline uint64_t PointCloud::Protocol::get_size_num() noexcept {
1392 uint64_t target_num = 0;
1394 uint64_t key_shift =
sizeof...(T) * 4;
1399 target_num |= (
static_cast<uint64_t
>(
sizeof(type)) & 0xF) << key_shift;
1406 template <
typename... T>
1407 inline uint64_t PointCloud::Protocol::get_type_num() noexcept {
1408 uint64_t target_num = 0;
1410 uint64_t key_shift =
sizeof...(T) * 4;
1415 target_num |= (
static_cast<uint64_t
>(get_type<decltype(type)>()) & 0xF) << key_shift;
1422 template <
typename T>
1423 inline constexpr uint8_t PointCloud::Protocol::get_type() noexcept {
1424 if constexpr (std::is_same_v<T, bool>) {
1426 }
else if constexpr (std::is_same_v<T, int8_t>) {
1428 }
else if constexpr (std::is_same_v<T, uint8_t>) {
1430 }
else if constexpr (std::is_same_v<T, int16_t>) {
1432 }
else if constexpr (std::is_same_v<T, uint16_t>) {
1434 }
else if constexpr (std::is_same_v<T, int32_t>) {
1436 }
else if constexpr (std::is_same_v<T, uint32_t>) {
1438 }
else if constexpr (std::is_same_v<T, int64_t>) {
1440 }
else if constexpr (std::is_same_v<T, uint64_t>) {
1442 }
else if constexpr (std::is_same_v<T, float>) {
1444 }
else if constexpr (std::is_same_v<T, double>) {
1451 inline constexpr uint64_t PointCloud::Protocol::get_pack_size() const noexcept {
1454 auto target_num = size_num;
1456 while (target_num > 0) {
1457 sum += target_num & 0xF;
1464 template <
typename T>
1465 inline bool PointCloud::xyz_in_extent(T x, T y, T z, uint16_t extent) noexcept {
1466 return x > -extent && x < extent && y > -extent && y < extent && z > -extent && z < extent;
static uint8_t * bytes_malloc(size_t size) noexcept
Allocates a raw aligned byte buffer through the global memory pool.
static void bytes_free(uint8_t *ptr, size_t size) noexcept
Returns a buffer previously obtained from bytes_malloc to the pool.
#define VUNLIKELY(...)
Short alias for VLINK_UNLIKELY.
Definition: macros.h:292
#define VLIKELY(...)
Short alias for VLINK_LIKELY.
Definition: macros.h:284
constexpr bool is_valid() noexcept
Definition: name_detector.h:382
@ kUnknownType
Unsupported type; is_supported() returns false.
Definition: serializer.h:157
size_t get_serialized_size(const T &src) noexcept
Returns the exact SOME/IP payload size for src.
Definition: someip_serializer.h:1426
VLINK_EXPORT std::ostream & operator<<(std::ostream &ostream, const BasePtr &status) noexcept
Writes the human-readable description of status to ostream.
struct VLINK_EXPORT_AND_ALIGNED(8) AudioFrame final
Definition: audio_frame.h:140
Definition: point_cloud.h:196
256-byte POD container holding a schema-described array of N-field point records.