72 const float &raw_distance,
74 const uint16_t rotation,
79 double distance_corr_x = 0;
80 double distance_corr_y = 0;
104 double distance_x = raw_distance + distance_corr_x;
107 x = xy_distance * sin_rot_angle
110 double distance_y = raw_distance + distance_corr_y;
113 y = xy_distance * cos_rot_angle
119 point->set_x(
static_cast<float>(-y));
120 point->set_y(
static_cast<float>(-x));
121 point->set_z(
static_cast<float>(z));
127 const float &raw_distance,
129 const double rotation,
134 double distance_corr_x = 0;
135 double distance_corr_y = 0;
136 double sinTheta_1[128] = {0};
137 double cosTheta_1[128] = {0};
138 double sinTheta_2[128] = {0};
139 double cosTheta_2[128] = {0};
150 for (
int i = 0; i < 128; i++) {
151 sinTheta_1[i] = sin(big_angle[i / 4] * M_PI / 180);
152 cosTheta_1[i] = cos(big_angle[i / 4] * M_PI / 180);
156 sinTheta_2[i] = sin((i % 4) * (-0.17) * M_PI / 180);
157 cosTheta_2[i] = cos((i % 4) * (-0.17) * M_PI / 180);
159 sinTheta_2[i] = sin(
prism_angle[i % 4] * M_PI / 180);
160 cosTheta_2[i] = cos(
prism_angle[i % 4] * M_PI / 180);
167 double cos_azimuth_half = cos(rotation * 0.5);
173 = sin_scan_laser_altitude[Laser_ring / 4 + 2]
174 + 2 * cos_azimuth_half
175 * sin_scan_mirror_altitude[Laser_ring % 4];
178 = sin_scan_laser_altitude[Laser_ring / 4 + 2]
179 + 2 * cos_azimuth_half
182 }
else if (Type ==
CH32) {
184 = sin_scan_laser_altitude[Laser_ring / 4]
185 + 2 * cos_azimuth_half
186 * sin_scan_mirror_altitude[Laser_ring % 4];
187 }
else if (Type ==
CH64) {
188 if (Laser_ring % 8 == 0 || Laser_ring % 8 == 1 || Laser_ring % 8 == 2
189 || Laser_ring % 8 == 3) {
196 Laser_ring % 8 == 4 || Laser_ring % 8 == 5
197 || Laser_ring % 8 == 6 || Laser_ring % 8 == 7) {
204 }
else if (Type ==
CH64w) {
205 if (Laser_ring / 4 % 2 == 0) {
206 cos_xita = cos((rotation / 2.0 + 22.5) * M_PI / 180);
208 cos_xita = cos((-rotation / 2.0 + 112.5) * M_PI / 180);
213 cos_theat = sqrt(1 - pow(sin_theat, 2));
214 cos_H_xita = (2 * _R_ *
theat2_c[Laser_ring] * cos_xita
217 sin_H_xita = sqrt(1 - pow(cos_H_xita, 2));
219 if (Laser_ring / 4 % 2 == 0) {
220 cos_xita_F = (cos_H_xita + sin_H_xita) * sqrt(0.5);
223 cos_xita_F = (cos_H_xita + sin_H_xita) * (-sqrt(0.5));
226 }
else if (Type ==
CH120) {
228 }
else if (Type ==
CH128) {
229 if (Laser_ring / 4 % 2 == 0) {
230 cos_azimuth_half = sin((rotation - 15 *
DEG_TO_RAD) * 0.5);
232 cos_azimuth_half = cos((rotation + 15 *
DEG_TO_RAD) * 0.5);
235 = sin_scan_laser_altitude_ch128[Laser_ring / 4]
236 + 2 * cos_azimuth_half * sinTheta_2[Laser_ring];
238 double _R_ = cosTheta_2[Laser_ring] * cosTheta_1[Laser_ring]
240 - sinTheta_2[Laser_ring] * sinTheta_1[Laser_ring];
242 = sinTheta_1[Laser_ring] + 2 * _R_ * sinTheta_2[Laser_ring];
262 double distance_x = raw_distance + distance_corr_x;
265 x = xy_distance * sin_rot_angle
268 double distance_y = raw_distance + distance_corr_y;
271 y = xy_distance * cos_rot_angle
277 point->set_x(
static_cast<float>(-y));
278 point->set_y(
static_cast<float>(-x));
279 point->set_z(
static_cast<float>(z));