171 #include <string_view>
172 #include <type_traits>
173 #include <unordered_map>
176 #include "../base/bytes.h"
177 #include "../base/quantize.h"
199 enum Type : uint8_t {
227 using KeyMap = std::unordered_map<std::string, uint16_t>;
232 using KeyList = std::vector<Key>;
255 explicit Vector3f(
float _x,
float _y,
float _z) noexcept;
288 explicit Vector3d(
double _x,
double _y,
double _z) noexcept;
346 bool operator<<(const Bytes& bytes) noexcept;
354 bool operator>>(Bytes& bytes) const noexcept;
362 [[nodiscard]] static
bool check_valid(const Bytes& bytes) noexcept;
369 [[nodiscard]]
bool is_valid() const noexcept;
377 bool shallow_copy(const
PointCloud& target) noexcept;
385 bool deep_copy(const
PointCloud& target) noexcept;
409 [[nodiscard]] KeyMap get_key_map(KeyList* key_list =
nullptr) const noexcept;
416 [[nodiscard]]
size_t size() const noexcept;
423 [[nodiscard]]
size_t pack_size() const noexcept;
430 [[nodiscard]]
bool is_owner() const noexcept;
437 [[nodiscard]] uint16_t get_extent() const noexcept;
444 [[nodiscard]]
bool get_vertical() const noexcept;
456 void set_vertical(
bool vertical) noexcept;
463 [[nodiscard]] uint8_t get_downsample() const noexcept;
477 bool downsample(uint8_t level) noexcept;
484 [[nodiscard]] uint64_t get_protocol_size_num() const noexcept;
491 [[nodiscard]] uint64_t get_protocol_type_num() const noexcept;
498 [[nodiscard]] std::
string get_protocol_size_str() const noexcept;
506 [[nodiscard]] std::
string get_protocol_name_str() const noexcept;
513 [[nodiscard]] std::
string get_protocol_type_str() const noexcept;
520 [[nodiscard]] const uint8_t* get_internal_data() const noexcept;
527 [[nodiscard]]
size_t get_reserved_size() const noexcept;
538 bool get_value_v3f(
float& x,
float& y,
float& z,
size_t loop_index) const noexcept;
547 bool get_value_v3f(
Vector3f& v3f,
size_t loop_index) const noexcept;
555 [[nodiscard]]
Vector3f get_value_v3f(
size_t loop_index) const noexcept;
566 bool get_value_v3d(
double& x,
double& y,
double& z,
size_t loop_index) const noexcept;
575 bool get_value_v3d(
Vector3d& v3d,
size_t loop_index) const noexcept;
583 [[nodiscard]]
Vector3d get_value_v3d(
size_t loop_index) const noexcept;
594 template <typename T>
595 bool get_value(T& t,
size_t loop_index, uint16_t offset) const noexcept;
605 template <typename T>
606 [[nodiscard]] T get_value(
size_t loop_index, uint16_t offset) const noexcept;
618 template <typename T>
619 bool get_value(T& t,
size_t loop_index, KeyMap& key_map, std::string_view key) const noexcept;
630 template <typename T>
631 [[nodiscard]] T get_value(
size_t loop_index, KeyMap& key_map, std::string_view key) const noexcept;
641 [[nodiscard]]
double get_value_for_double_float(
size_t loop_index, uint16_t offset, uint8_t type) const noexcept;
652 [[nodiscard]]
double get_value_for_double_float(
size_t loop_index, KeyMap& key_map, std::string_view key,
653 uint8_t type) const noexcept;
663 [[nodiscard]] std::
string get_value_for_print(
size_t loop_index, uint16_t offset, uint8_t type) const noexcept;
674 [[nodiscard]] std::
string get_value_for_print(
size_t loop_index, KeyMap& key_map, std::string_view key,
675 uint8_t type) const noexcept;
694 bool create(
size_t size, uint64_t size_num, uint64_t type_num, std::string_view key_str, uint16_t extent = 0,
695 bool vertical = false) noexcept;
707 template <typename... T>
708 bool create(
size_t _size, const std::vector<std::
string>& keys = {}, uint16_t extent = 0,
709 bool vertical =
false) noexcept;
721 template <
typename... T>
722 bool create_v3f(
size_t _size,
const std::vector<std::string>& keys = {}, uint16_t extent = 0,
723 bool vertical =
false) noexcept;
735 template <
typename... T>
736 bool create_v3d(
size_t _size,
const std::vector<std::string>& keys = {}, uint16_t extent = 0,
737 bool vertical =
false) noexcept;
746 bool fill_packed_data(
const uint8_t* src_data,
size_t _size) noexcept;
755 template <
typename... T>
756 bool push_value(T... args) noexcept;
769 template <
typename... T>
770 bool push_value_v3f(
float x,
float y,
float z, T... args) noexcept;
781 template <
typename... T>
782 bool push_value_v3f(
Vector3f v3f, T... args) noexcept;
795 template <
typename... T>
796 bool push_value_v3d(
double x,
double y,
double z, T... args) noexcept;
807 template <
typename... T>
808 bool push_value_v3d(
Vector3d v3d, T... args) noexcept;
820 bool resize(
size_t size) noexcept;
834 template <
typename... T>
835 bool set_value(
size_t loop_index, T... args) noexcept;
849 template <
typename... T>
850 bool set_value_v3f(
size_t loop_index,
float x,
float y,
float z, T... args) noexcept;
862 template <
typename... T>
863 bool set_value_v3f(
size_t loop_index,
Vector3f v3f, T... args) noexcept;
877 template <
typename... T>
878 bool set_value_v3d(
size_t loop_index,
double x,
double y,
double z, T... args) noexcept;
890 template <
typename... T>
891 bool set_value_v3d(
size_t loop_index,
Vector3d v3d, T... args) noexcept;
904 void clear(
bool force =
false) noexcept;
916 uint32_t& get_reserved() noexcept {
return reserved_buf_; }
928 uint32_t& get_reserved2() noexcept {
return reserved_buf2_; }
940 uint8_t& get_reserved3() noexcept {
return reserved_buf3_; }
944 static constexpr
bool kZerocopyTypes{
true};
948 uint64_t size_num{0};
949 uint64_t type_num{0};
952 Protocol() noexcept = default;
954 template <typename... T>
955 static uint64_t get_size_num() noexcept;
957 template <typename... T>
958 static uint64_t get_type_num() noexcept;
960 template <typename T>
961 static constexpr uint8_t get_type() noexcept;
963 constexpr uint64_t get_pack_size() const noexcept;
965 static
bool check_valid(uint64_t _size_num, std::string_view _names) noexcept;
967 static std::
string get_names(const std::vector<std::
string>& keys) noexcept;
969 KeyList get_key_list() const noexcept;
971 std::
string get_size_for_print() const noexcept;
973 std::
string get_type_for_print() const noexcept;
976 template <typename T>
977 static
bool xyz_in_extent(T x, T y, T z, uint16_t extent) noexcept;
979 bool compress_protocol_xyz() noexcept;
982 uint8_t* data_{
nullptr};
985 uint32_t reserved_buf_{0};
986 uint32_t reserved_buf2_{0};
987 uint16_t pack_size_{0};
989 uint8_t downsample_{0};
990 uint8_t reserved_buf3_{0};
991 bool vertical_{
false};
992 bool is_owner_{
false};
995 static constexpr uint32_t kMagicNumberBegin{0x98B7F16A};
996 static constexpr uint32_t kMagicNumberEnd{0x98B7F16F};
1003 template <
typename T>
1004 inline bool PointCloud::get_value(T& t,
size_t loop_index, uint16_t offset)
const noexcept {
1005 static_assert(std::is_fundamental_v<T>,
"T must be fundamental.");
1007 if (extent_ != 0 && offset <
sizeof(int16_t) * 3) {
1008 if constexpr (std::is_same_v<T, float> || std::is_same_v<T, double>) {
1011 size_t p = (loop_index * pack_size_) + offset;
1013 if VUNLIKELY (p +
sizeof(value) > size_ * pack_size_) {
1018 std::memcpy(&value, data_ + p,
sizeof(value));
1019 t = Quantize::decode<T>(extent_, value);
1025 size_t p = (loop_index * pack_size_) + offset;
1027 if VUNLIKELY (p +
sizeof(T) > size_ * pack_size_) {
1028 std::memset(&t, 0,
sizeof(T));
1032 std::memcpy(&t, data_ + p,
sizeof(T));
1037 template <
typename T>
1038 inline T PointCloud::get_value(
size_t loop_index, uint16_t offset)
const noexcept {
1039 static_assert(std::is_fundamental_v<T>,
"T must be fundamental.");
1043 get_value(t, loop_index, offset);
1048 template <
typename T>
1049 inline bool PointCloud::get_value(T& t,
size_t loop_index, KeyMap& key_map, std::string_view key)
const noexcept {
1050 static_assert(std::is_fundamental_v<T>,
"T must be fundamental.");
1052 auto iter = key_map.find(key.data());
1055 std::memset(&t, 0,
sizeof(T));
1059 return get_value<T>(t, loop_index, iter->second);
1062 template <
typename T>
1063 inline T PointCloud::get_value(
size_t loop_index, KeyMap& key_map, std::string_view key)
const noexcept {
1064 static_assert(std::is_fundamental_v<T>,
"T must be fundamental.");
1068 get_value(t, loop_index, key_map, key);
1073 template <
typename... T>
1074 inline bool PointCloud::create(
size_t size,
const std::vector<std::string>& keys, uint16_t extent,
1075 bool vertical) noexcept {
1076 static_assert((std::is_fundamental_v<T> && ...),
"All types must be fundamental.");
1078 static_assert(
sizeof...(T) >= 3 &&
sizeof...(T) <= 16,
"The number of keys ranges is [3 ~ 16].");
1080 if VUNLIKELY (
sizeof...(T) != keys.size()) {
1084 uint64_t size_num = Protocol::get_size_num<T...>();
1090 std::string key_str = Protocol::get_names(keys);
1096 uint64_t type_num = Protocol::get_type_num<T...>();
1102 Protocol new_protocol{};
1103 new_protocol.size_num = size_num;
1104 std::memset(new_protocol.names, 0,
sizeof(new_protocol.names));
1105 std::memcpy(new_protocol.names, key_str.c_str(), key_str.size());
1106 new_protocol.type_num = type_num;
1110 protocol_probe.protocol_ = new_protocol;
1112 if VUNLIKELY (!protocol_probe.compress_protocol_xyz()) {
1116 new_protocol = protocol_probe.protocol_;
1119 if (is_owner_ && data_ && capacity_ != 0) {
1129 protocol_ = new_protocol;
1131 vertical_ = vertical;
1134 pack_size_ = protocol_.get_pack_size();
1135 capacity_ = size * pack_size_;
1145 template <
typename... T>
1146 inline bool PointCloud::create_v3f(
size_t _size,
const std::vector<std::string>& keys, uint16_t extent,
1147 bool vertical) noexcept {
1148 std::vector<std::string> target_keys{
"x",
"y",
"z"};
1150 target_keys.insert(target_keys.end(), keys.begin(), keys.end());
1152 return create<float, float, float, T...>(_size, target_keys, extent, vertical);
1155 template <
typename... T>
1156 inline bool PointCloud::create_v3d(
size_t _size,
const std::vector<std::string>& keys, uint16_t extent,
1157 bool vertical) noexcept {
1158 std::vector<std::string> target_keys{
"x",
"y",
"z"};
1160 target_keys.insert(target_keys.end(), keys.begin(), keys.end());
1162 return create<double, double, double, T...>(_size, target_keys, extent, vertical);
1165 inline bool PointCloud::fill_packed_data(
const uint8_t* src_data,
size_t _size) noexcept {
1166 bool is_fill_success =
false;
1168 if VUNLIKELY (_size == 0 || !src_data) {
1169 is_fill_success =
false;
1170 }
else if VUNLIKELY (!is_owner_ || !data_ || pack_size_ == 0 || capacity_ == 0) {
1171 is_fill_success =
false;
1172 }
else if VUNLIKELY (_size * pack_size_ > capacity_) {
1173 is_fill_success =
false;
1175 std::memcpy(data_, src_data, _size * pack_size_);
1177 index_ = _size * pack_size_;
1178 is_fill_success =
true;
1181 return is_fill_success;
1184 template <
typename... T>
1185 inline bool PointCloud::push_value(T... args) noexcept {
1186 static_assert((std::is_fundamental_v<T> && ...),
"All types must be fundamental.");
1188 static_assert(
sizeof...(T) >= 3 &&
sizeof...(T) <= 16,
"The number of keys ranges is [3 ~ 16].");
1190 if VUNLIKELY (!is_owner_ || !data_ || pack_size_ == 0 || capacity_ == 0) {
1194 if VUNLIKELY (size_ * pack_size_ >= capacity_) {
1198 constexpr
size_t kTargetPackSize = (
sizeof(T) + ...);
1200 if VUNLIKELY (kTargetPackSize != pack_size_) {
1204 auto* target_ptr = data_ + index_;
1208 std::memcpy(target_ptr, &args,
sizeof(args));
1209 target_ptr +=
sizeof(args);
1213 index_ += pack_size_;
1219 template <
typename... T>
1220 inline bool PointCloud::push_value_v3f(
float x,
float y,
float z, T... args) noexcept {
1222 if VUNLIKELY (!xyz_in_extent(x, y, z, extent_)) {
1226 auto qx = Quantize::encode<int16_t>(extent_, x);
1227 auto qy = Quantize::encode<int16_t>(extent_, y);
1228 auto qz = Quantize::encode<int16_t>(extent_, z);
1230 return push_value(qx, qy, qz, args...);
1233 return push_value(x, y, z, args...);
1236 template <
typename... T>
1237 inline bool PointCloud::push_value_v3f(
Vector3f v3f, T... args) noexcept {
1238 return push_value_v3f(v3f.x, v3f.y, v3f.z, args...);
1241 template <
typename... T>
1242 inline bool PointCloud::push_value_v3d(
double x,
double y,
double z, T... args) noexcept {
1244 if VUNLIKELY (!xyz_in_extent(x, y, z, extent_)) {
1248 auto qx = Quantize::encode<int16_t>(extent_, x);
1249 auto qy = Quantize::encode<int16_t>(extent_, y);
1250 auto qz = Quantize::encode<int16_t>(extent_, z);
1252 return push_value(qx, qy, qz, args...);
1255 return push_value(x, y, z, args...);
1258 template <
typename... T>
1259 inline bool PointCloud::push_value_v3d(
Vector3d v3d, T... args) noexcept {
1260 return push_value_v3d(v3d.x, v3d.y, v3d.z, args...);
1263 inline bool PointCloud::resize(
size_t size) noexcept {
1264 if VUNLIKELY (!is_owner_ || !data_ || pack_size_ == 0 || capacity_ == 0) {
1268 if VUNLIKELY (size * pack_size_ > capacity_) {
1273 index_ = size_ * pack_size_;
1278 template <
typename... T>
1279 bool PointCloud::set_value(
size_t loop_index, T... args) noexcept {
1280 static_assert((std::is_fundamental_v<T> && ...),
"All types must be fundamental.");
1282 static_assert(
sizeof...(T) >= 3 &&
sizeof...(T) <= 16,
"The number of keys ranges is [3 ~ 16].");
1284 if VUNLIKELY (!is_owner_ || !data_ || pack_size_ == 0 || capacity_ == 0) {
1288 if VUNLIKELY (loop_index >= size_ || size_ * pack_size_ > capacity_) {
1292 if VUNLIKELY (index_ != size_ * pack_size_) {
1293 std::cerr <<
"[PointCloud::set_value] Invalid buffer state: "
1294 <<
"The current buffer size does not match the capacity. "
1295 <<
"Please call resize(size) before using set_value()." << std::endl;
1299 constexpr
size_t kTargetPackSize = (
sizeof(T) + ...);
1301 if VUNLIKELY (kTargetPackSize != pack_size_) {
1305 auto* target_ptr = data_ + (loop_index * pack_size_);
1309 std::memcpy(target_ptr, &args,
sizeof(args));
1310 target_ptr +=
sizeof(args);
1317 template <
typename... T>
1318 inline bool PointCloud::set_value_v3f(
size_t loop_index,
float x,
float y,
float z, T... args) noexcept {
1320 if VUNLIKELY (!xyz_in_extent(x, y, z, extent_)) {
1324 auto qx = Quantize::encode<int16_t>(extent_, x);
1325 auto qy = Quantize::encode<int16_t>(extent_, y);
1326 auto qz = Quantize::encode<int16_t>(extent_, z);
1328 return set_value(loop_index, qx, qy, qz, args...);
1331 return set_value(loop_index, x, y, z, args...);
1334 template <
typename... T>
1335 inline bool PointCloud::set_value_v3f(
size_t loop_index,
Vector3f v3f, T... args) noexcept {
1336 return set_value_v3f(loop_index, v3f.x, v3f.y, v3f.z, args...);
1339 template <
typename... T>
1340 inline bool PointCloud::set_value_v3d(
size_t loop_index,
double x,
double y,
double z, T... args) noexcept {
1342 if VUNLIKELY (!xyz_in_extent(x, y, z, extent_)) {
1346 auto qx = Quantize::encode<int16_t>(extent_, x);
1347 auto qy = Quantize::encode<int16_t>(extent_, y);
1348 auto qz = Quantize::encode<int16_t>(extent_, z);
1350 return set_value(loop_index, qx, qy, qz, args...);
1353 return set_value(loop_index, x, y, z, args...);
1356 template <
typename... T>
1357 inline bool PointCloud::set_value_v3d(
size_t loop_index,
Vector3d v3d, T... args) noexcept {
1358 return set_value_v3d(loop_index, v3d.x, v3d.y, v3d.z, args...);
1361 template <
typename... T>
1362 inline uint64_t PointCloud::Protocol::get_size_num() noexcept {
1363 uint64_t target_num = 0;
1365 uint64_t key_shift =
sizeof...(T) * 4;
1370 target_num |= (
static_cast<uint64_t
>(
sizeof(type)) & 0xF) << key_shift;
1377 template <
typename... T>
1378 inline uint64_t PointCloud::Protocol::get_type_num() noexcept {
1379 uint64_t target_num = 0;
1381 uint64_t key_shift =
sizeof...(T) * 4;
1386 target_num |= (
static_cast<uint64_t
>(get_type<decltype(type)>()) & 0xF) << key_shift;
1393 template <
typename T>
1394 inline constexpr uint8_t PointCloud::Protocol::get_type() noexcept {
1395 if constexpr (std::is_same_v<T, bool>) {
1397 }
else if constexpr (std::is_same_v<T, int8_t>) {
1399 }
else if constexpr (std::is_same_v<T, uint8_t>) {
1401 }
else if constexpr (std::is_same_v<T, int16_t>) {
1403 }
else if constexpr (std::is_same_v<T, uint16_t>) {
1405 }
else if constexpr (std::is_same_v<T, int32_t>) {
1407 }
else if constexpr (std::is_same_v<T, uint32_t>) {
1409 }
else if constexpr (std::is_same_v<T, int64_t>) {
1411 }
else if constexpr (std::is_same_v<T, uint64_t>) {
1413 }
else if constexpr (std::is_same_v<T, float>) {
1415 }
else if constexpr (std::is_same_v<T, double>) {
1422 inline constexpr uint64_t PointCloud::Protocol::get_pack_size() const noexcept {
1425 auto target_num = size_num;
1427 while (target_num > 0) {
1428 sum += target_num & 0xF;
1435 template <
typename T>
1436 inline bool PointCloud::xyz_in_extent(T x, T y, T z, uint16_t extent) noexcept {
1437 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:289
#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:155
size_t get_serialized_size(const T &src) noexcept
Returns a serialised-size hint for src with explicit codec tag.
Definition: serializer-inl.h:298
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:195
256-byte POD container holding a schema-described array of N-field point records.