Apollo 10.0
自动驾驶开放平台
lslidar_parser.cc
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2017 The Apollo Authors. All Rights Reserved.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *****************************************************************************/
16
18
19namespace apollo {
20namespace drivers {
21namespace lslidar {
22namespace parser {
23
25 last_time_stamp_(0), config_(config) {
26 for (int i = 0; i < 4; ++i) {
27 prism_angle64[i] = i * 0.35;
28 }
29 for (int j = 0; j < 128; ++j) {
30 // 右边
31 if (j / 4 % 2 == 0) {
32 theat1_s[j] = sin((-25 + floor(j / 8) * 2.5) * M_PI / 180);
33 theat2_s[j] = sin(prism_angle64[j % 4] * M_PI / 180);
34 theat1_c[j] = cos((-25 + floor(j / 8) * 2.5) * M_PI / 180);
35 theat2_c[j] = cos(prism_angle64[j % 4] * M_PI / 180);
36 } else { // 左边
37 theat1_s[j] = sin((-24 + floor(j / 8) * 2.5) * M_PI / 180);
38 theat2_s[j] = sin(prism_angle64[j % 4] * M_PI / 180);
39 theat1_c[j] = cos((-24 + floor(j / 8) * 2.5) * M_PI / 180);
40 theat2_c[j] = cos(prism_angle64[j % 4] * M_PI / 180);
41 }
42 }
43}
44
45bool LslidarParser::is_scan_valid(int rotation, float range) {
46 // check range first
47 if (range < config_.min_range() || range > config_.max_range())
48 return false;
49 else
50 return true;
51}
52
55 if (config_.calibration()) {
57
59 AFATAL << "Unable to open calibration file: "
61 }
62 }
63 for (uint16_t rot_index = 0; rot_index < ROTATION_MAX_UNITS; ++rot_index) {
64 float rotation_f = ROTATION_RESOLUTION
65 * static_cast<float>(rot_index * M_PI) / 180.0f;
66 cos_azimuth_table[rot_index] = cosf(rotation_f);
67 sin_azimuth_table[rot_index] = sinf(rotation_f);
68 }
69}
70
72 const float &raw_distance,
73 LaserCorrection *corrections,
74 const uint16_t rotation,
75 PointXYZIT *point) {
76 double x = 0.0;
77 double y = 0.0;
78 double z = 0.0;
79 double distance_corr_x = 0;
80 double distance_corr_y = 0;
81
82 corrections->cos_rot_correction = cosf(corrections->rot_correction);
83 corrections->sin_rot_correction = sinf(corrections->rot_correction);
84 corrections->cos_vert_correction = cosf(corrections->vert_correction);
85 corrections->sin_vert_correction = sinf(corrections->vert_correction);
86
87 double distance = raw_distance + corrections->dist_correction;
88
89 double cos_rot_angle
90 = cos_azimuth_table[rotation] * corrections->cos_rot_correction
91 + sin_azimuth_table[rotation] * corrections->sin_rot_correction;
92 double sin_rot_angle
93 = sin_azimuth_table[rotation] * corrections->cos_rot_correction
94 - cos_azimuth_table[rotation] * corrections->sin_rot_correction;
95
96 // Compute the distance in the xy plane (w/o accounting for rotation)
97 double xy_distance = distance * corrections->cos_vert_correction;
98
99 // Get 2points calibration values,Linear interpolation to get distance
100 // correction for X and Y, that means distance correction use
101 // different value at different distance
102 distance_corr_x = distance_corr_y = corrections->dist_correction;
103
104 double distance_x = raw_distance + distance_corr_x;
105 xy_distance = distance_x * corrections->cos_vert_correction;
106
107 x = xy_distance * sin_rot_angle
108 - corrections->horiz_offset_correction * cos_rot_angle;
109
110 double distance_y = raw_distance + distance_corr_y;
111 xy_distance = distance_y * corrections->cos_vert_correction;
112
113 y = xy_distance * cos_rot_angle
114 + corrections->horiz_offset_correction * sin_rot_angle;
115 z = distance * corrections->sin_vert_correction
116 + corrections->vert_offset_correction;
117
119 point->set_x(static_cast<float>(-y));
120 point->set_y(static_cast<float>(-x));
121 point->set_z(static_cast<float>(z));
122}
123
125 int Laser_ring,
126 int Type,
127 const float &raw_distance,
128 LaserCorrection *corrections,
129 const double rotation,
130 PointXYZIT *point) {
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
154 if (abs(prism_angle[0]) < 1e-6 && abs(prism_angle[1]) < 1e-6
155 && abs(prism_angle[2]) < 1e-6 && abs(prism_angle[3]) < 1e-6) {
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 {
159 sinTheta_2[i] = sin(prism_angle[i % 4] * M_PI / 180);
160 cosTheta_2[i] = cos(prism_angle[i % 4] * M_PI / 180);
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
169 if (Type == CH16) {
170 if (abs(prism_angle[0]) < 1e-6 && abs(prism_angle[1]) < 1e-6
171 && abs(prism_angle[2]) < 1e-6 && abs(prism_angle[3]) < 1e-6) {
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
180 * sin(prism_angle[Laser_ring % 4] * M_PI / 180);
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
191 = sin(-13.33 * DEG_TO_RAD
192 + floor(Laser_ring / 4) * 1.33 * DEG_TO_RAD)
193 + 2 * cos(rotation / 2 + 1.05 * DEG_TO_RAD)
194 * sin((Laser_ring % 4) * 0.33 * DEG_TO_RAD);
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
199 = sin(-13.33 * DEG_TO_RAD
200 + floor(Laser_ring / 4) * 1.33 * DEG_TO_RAD)
201 + 2 * cos(rotation / 2 - 1.05 * DEG_TO_RAD)
202 * sin((Laser_ring % 4) * 0.33 * DEG_TO_RAD);
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 }
210 _R_ = theat2_c[Laser_ring] * theat1_c[Laser_ring] * cos_xita
211 - theat2_s[Laser_ring] * theat1_s[Laser_ring];
212 sin_theat = theat1_s[Laser_ring] + 2 * _R_ * theat2_s[Laser_ring];
213 cos_theat = sqrt(1 - pow(sin_theat, 2));
214 cos_H_xita = (2 * _R_ * theat2_c[Laser_ring] * cos_xita
215 - theat1_c[Laser_ring])
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) {
230 cos_azimuth_half = sin((rotation - 15 * DEG_TO_RAD) * 0.5);
231 } else {
232 cos_azimuth_half = cos((rotation + 15 * DEG_TO_RAD) * 0.5);
233 }
234 corrections->sin_vert_correction
235 = sin_scan_laser_altitude_ch128[Laser_ring / 4]
236 + 2 * cos_azimuth_half * sinTheta_2[Laser_ring];
237 } else if (Type == CH128X1) {
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 // Compute the distance in the xy plane (w/o accounting for rotation)
255 double xy_distance = distance * corrections->cos_vert_correction;
256
257 // Get 2points calibration values,Linear interpolation to get distance
258 // correction for X and Y, that means distance correction use
259 // different value at different distance
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}
281
283 Config config = source_config;
284
285 if (config.model() == LSLIDAR16P) {
286 return new Lslidar16Parser(config);
287 } else if (config.model() == LSLIDAR32P) {
288 return new Lslidar32Parser(config);
289 } else if (
290 config.model() == LSLIDAR_C32_V4 || config.model() == LSLIDAR_C16_V4
291 || config.model() == LSLIDAR_C8_V4
292 || config.model() == LSLIDAR_C1_V4) {
293 return new LslidarCXV4Parser(config);
294 } else if (config.model() == LSLIDAR_CH16) {
295 return new LslidarCH16Parser(config);
296 } else if (config.model() == LSLIDAR_CH32) {
297 return new LslidarCH32Parser(config);
298 } else if (config.model() == LSLIDAR_CH64) {
299 return new LslidarCH64Parser(config);
300 } else if (config.model() == LSLIDAR_CH64w) {
301 return new LslidarCH64wParser(config);
302 } else if (config.model() == LSLIDAR_CH120) {
303 return new LslidarCH120Parser(config);
304 } else if (config.model() == LSLIDAR_CH128) {
305 return new LslidarCH128Parser(config);
306 } else if (config.model() == LSLIDAR_CH128X1) {
307 return new LslidarCH128X1Parser(config);
308 } else if (config.model() == LSLIDAR_LS128S2) {
309 return new LslidarLS128S2Parser(config);
310 } else {
311 AERROR << "invalid model";
312 return nullptr;
313 }
314}
315
316} // namespace parser
317} // namespace lslidar
318} // namespace drivers
319} // namespace apollo
void read(const std::string &calibration_file)
static LslidarParser * CreateParser(Config config)
virtual void setup()
Set up for on-line operation.
void ComputeCoords2(int Laser_ring, int Type, const float &raw_distance, LaserCorrection *corrections, const double rotation, PointXYZIT *point)
bool is_scan_valid(int rotation, float distance)
void ComputeCoords(const float &raw_distance, LaserCorrection *corrections, const uint16_t rotation, PointXYZIT *point)
#define AERROR
Definition log.h:44
#define AFATAL
Definition log.h:45
These classes Unpack raw Lslidar LIDAR packets into several useful formats.
#define DEG_TO_RAD
#define CH120
#define CH32
#define CH16
#define CH64
#define CH128
#define CH128X1
#define CH64w
class register implement
Definition arena_queue.h:37
correction values for a single laser
Definition calibration.h:45
float sin_rot_correction
cached sine of rot_correction
Definition calibration.h:62
float sin_vert_correction
cached sine of vert_correction
Definition calibration.h:64
float cos_vert_correction
cached cosine of vert_correction
Definition calibration.h:63
float cos_rot_correction
cached values calculated when the calibration file is read
Definition calibration.h:61