forked from hku-mars/FAST_LIO
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathpreprocess.h
124 lines (109 loc) · 3.58 KB
/
preprocess.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
#include <ros/ros.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <livox_ros_driver/CustomMsg.h>
using namespace std;
#define IS_VALID(a) ((abs(a)>1e8) ? true : false)
typedef pcl::PointXYZINormal PointType;
typedef pcl::PointCloud<PointType> PointCloudXYZI;
enum LID_TYPE{AVIA = 1, VELO16, OUST64}; //{1, 2, 3}
enum TIME_UNIT{SEC = 0, MS = 1, US = 2, NS = 3};
enum Feature{Nor, Poss_Plane, Real_Plane, Edge_Jump, Edge_Plane, Wire, ZeroPoint};
enum Surround{Prev, Next};
enum E_jump{Nr_nor, Nr_zero, Nr_180, Nr_inf, Nr_blind};
struct orgtype
{
double range;
double dista;
double angle[2];
double intersect;
E_jump edj[2];
Feature ftype;
orgtype()
{
range = 0;
edj[Prev] = Nr_nor;
edj[Next] = Nr_nor;
ftype = Nor;
intersect = 2;
}
};
namespace velodyne_ros {
struct EIGEN_ALIGN16 Point {
PCL_ADD_POINT4D;
float intensity;
float time;
uint16_t ring;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace velodyne_ros
POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point,
(float, x, x)
(float, y, y)
(float, z, z)
(float, intensity, intensity)
(float, time, time)
(uint16_t, ring, ring)
)
namespace ouster_ros {
struct EIGEN_ALIGN16 Point {
PCL_ADD_POINT4D;
float intensity;
uint32_t t;
uint16_t reflectivity;
uint8_t ring;
uint16_t ambient;
uint32_t range;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace ouster_ros
// clang-format off
POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point,
(float, x, x)
(float, y, y)
(float, z, z)
(float, intensity, intensity)
// use std::uint32_t to avoid conflicting with pcl::uint32_t
(std::uint32_t, t, t)
(std::uint16_t, reflectivity, reflectivity)
(std::uint8_t, ring, ring)
(std::uint16_t, ambient, ambient)
(std::uint32_t, range, range)
)
class Preprocess
{
public:
// EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Preprocess();
~Preprocess();
void process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out);
void process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out);
void set(bool feat_en, int lid_type, double bld, int pfilt_num);
// sensor_msgs::PointCloud2::ConstPtr pointcloud;
PointCloudXYZI pl_full, pl_corn, pl_surf;
PointCloudXYZI pl_buff[128]; //maximum 128 line lidar
vector<orgtype> typess[128]; //maximum 128 line lidar
float time_unit_scale;
int lidar_type, point_filter_num, N_SCANS, SCAN_RATE, time_unit;
double blind;
bool feature_enabled, given_offset_time;
ros::Publisher pub_full, pub_surf, pub_corn;
private:
void avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg);
void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
void velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
void give_feature(PointCloudXYZI &pl, vector<orgtype> &types);
void pub_func(PointCloudXYZI &pl, const ros::Time &ct);
int plane_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct);
bool small_plane(const PointCloudXYZI &pl, vector<orgtype> &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct);
bool edge_jump_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, Surround nor_dir);
int group_size;
double disA, disB, inf_bound;
double limit_maxmid, limit_midmin, limit_maxmin;
double p2l_ratio;
double jump_up_limit, jump_down_limit;
double cos160;
double edgea, edgeb;
double smallp_intersect, smallp_ratio;
double vx, vy, vz;
};