130 {
131 double x = 0.0;
132 double y = 0.0;
133 double z = 0.0;
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};
140 double cos_xita;
141
142 double sin_theat;
143 double cos_theat;
144 double _R_;
145
146 double cos_H_xita;
147 double sin_H_xita;
148 double cos_xita_F;
149
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);
153
156 sinTheta_2[i] =
sin((i % 4) * (-0.17) * M_PI / 180);
157 cosTheta_2[i] =
cos((i % 4) * (-0.17) * M_PI / 180);
158 } else {
161 }
162 }
163
164 corrections->cos_rot_correction = cosf(corrections->rot_correction);
165 corrections->sin_rot_correction = sinf(corrections->rot_correction);
166
167 double cos_azimuth_half =
cos(rotation * 0.5);
168
172 corrections->sin_vert_correction
173 = sin_scan_laser_altitude[Laser_ring / 4 + 2]
174 + 2 * cos_azimuth_half
175 * sin_scan_mirror_altitude[Laser_ring % 4];
176 } else {
177 corrections->sin_vert_correction
178 = sin_scan_laser_altitude[Laser_ring / 4 + 2]
179 + 2 * cos_azimuth_half
181 }
182 }
else if (Type ==
CH32) {
183 corrections->sin_vert_correction
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) {
190 corrections->sin_vert_correction
195 } else if (
196 Laser_ring % 8 == 4 || Laser_ring % 8 == 5
197 || Laser_ring % 8 == 6 || Laser_ring % 8 == 7) {
198 corrections->sin_vert_correction
203 }
204 }
else if (Type ==
CH64w) {
205 if (Laser_ring / 4 % 2 == 0) {
206 cos_xita =
cos((rotation / 2.0 + 22.5) * M_PI / 180);
207 } else {
208 cos_xita =
cos((-rotation / 2.0 + 112.5) * M_PI / 180);
209 }
213 cos_theat = sqrt(1 - pow(sin_theat, 2));
214 cos_H_xita = (2 * _R_ *
theat2_c[Laser_ring] * cos_xita
216 / cos_theat;
217 sin_H_xita = sqrt(1 - pow(cos_H_xita, 2));
218
219 if (Laser_ring / 4 % 2 == 0) {
220 cos_xita_F = (cos_H_xita + sin_H_xita) * sqrt(0.5);
221 corrections->sin_vert_correction = sqrt(1 - pow(cos_xita_F, 2));
222 } else {
223 cos_xita_F = (cos_H_xita + sin_H_xita) * (-sqrt(0.5));
224 corrections->sin_vert_correction = sqrt(1 - pow(cos_xita_F, 2));
225 }
226 }
else if (Type ==
CH120) {
227 corrections->sin_vert_correction = sinf(corrections->vert_correction);
228 }
else if (Type ==
CH128) {
229 if (Laser_ring / 4 % 2 == 0) {
231 } else {
233 }
234 corrections->sin_vert_correction
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]
239 * cos_azimuth_half
240 - sinTheta_2[Laser_ring] * sinTheta_1[Laser_ring];
241 corrections->sin_vert_correction
242 = sinTheta_1[Laser_ring] + 2 * _R_ * sinTheta_2[Laser_ring];
243 }
244 corrections->cos_vert_correction
245 = sqrt(1 - pow(corrections->sin_vert_correction, 2));
246
247 double distance = raw_distance + corrections->dist_correction;
248
249 double cos_rot_angle =
cos(rotation) * corrections->cos_rot_correction
250 +
sin(rotation) * corrections->sin_rot_correction;
251 double sin_rot_angle =
sin(rotation) * corrections->cos_rot_correction
252 -
cos(rotation) * corrections->sin_rot_correction;
253
254
255 double xy_distance = distance * corrections->cos_vert_correction;
256
257
258
259
260 distance_corr_x = distance_corr_y = corrections->dist_correction;
261
262 double distance_x = raw_distance + distance_corr_x;
263 xy_distance = distance_x * corrections->cos_vert_correction;
264
265 x = xy_distance * sin_rot_angle
266 - corrections->horiz_offset_correction * cos_rot_angle;
267
268 double distance_y = raw_distance + distance_corr_y;
269 xy_distance = distance_y * corrections->cos_vert_correction;
270
271 y = xy_distance * cos_rot_angle
272 + corrections->horiz_offset_correction * sin_rot_angle;
273 z = distance * corrections->sin_vert_correction
274 + corrections->vert_offset_correction;
275
277 point->set_x(static_cast<float>(-y));
278 point->set_y(static_cast<float>(-x));
279 point->set_z(static_cast<float>(z));
280}