#pragma once #include <string> #include <Eigen/Core> #include <Eigen/Geometry> #include "../utils/gnss.h" namespace juefx { #define VOXEL_RESOLUTION_1X 0 #define VOXEL_RESOLUTION_2X 1 #define VOXEL_RESOLUTION_4X 2 #define VOXEL_RESOLUTION_8X 3 #define VOXEL_RESOLUTION_16X 4 #define VOXEL_RESOLUTION_NUM 5 #define GRID_MOVE_LEFT 1 #define GRID_MOVE_RIGHT 2 #define GRID_MOVE_UP 4 #define GRID_MOVE_DOWN 8 struct GridIndex { struct SByteId { int8_t x = 0; int8_t y = 0; }; struct ByteId { uint8_t x = 0; uint8_t y = 0; }; struct ShortId { int16_t x = 0; int16_t y = 0; }; SByteId meshIndex; ByteId blockIndex; ShortId gridIndex; GridIndex() { meshIndex.x = -127; } inline GridIndex Move(int move_type) const { GridIndex ret = *this; if ((move_type & GRID_MOVE_LEFT) != 0) ret.gridIndex.x--; if ((move_type & GRID_MOVE_RIGHT) != 0) ret.gridIndex.x++; if ((move_type & GRID_MOVE_UP) != 0) ret.gridIndex.y++; if ((move_type & GRID_MOVE_DOWN) != 0) ret.gridIndex.y--; return ret; } inline GridIndex Move(int16_t x, int16_t y) const { GridIndex ret = *this; ret.gridIndex.x += x; ret.gridIndex.y += y; return ret; } bool IsValid() { return meshIndex.x != -127; } }; struct Distribution { int32_t floor; uint32_t height : 24; uint32_t intensity : 8; }; typedef std::vector<Distribution> GridData; class VoxelMap { public: bool SetMapPath(std::string path_name) { return true; } bool IsMapLoaded(const GnssPoint blh_pose) { return true; } bool GuessPosition(const GnssPoint &init_pose, Eigen::Isometry3d &pose_in_map) { return true; } void ConfirmPosition(const Eigen::Vector3d &pose_in_map, GnssPoint &final_pose) {} public: inline float GetResolution(uint8_t type) const { return 0; } inline const GridIndex GetCellIndex(uint8_t type, float x, float y) const { return GridIndex(); }; inline GridData GetCellValue(uint8_t type, GridIndex index) const { return GridData(); } inline Eigen::Vector2f GetCenterOfCell(uint8_t type, float x, float y) const { return Eigen::Vector2f::Zero(); } inline Eigen::Vector2f GetCenterOfCell(uint8_t type, GridIndex index) const { return Eigen::Vector2f::Zero(); } }; }