33 return originalReceivedTimestamp;
39 out << timestamp << sensorLabel << rowCount << columnCount
40 << rangeResolution << startAzimuth << azimuthSpan << sweepDuration
41 << lidarModel << minRange << maxRange << sensorPose
42 << originalReceivedTimestamp << has_satellite_timestamp;
44 out.WriteAs<uint16_t>(rangeImage.cols());
45 out.WriteAs<uint16_t>(rangeImage.rows());
46 if (!rangeImage.empty())
47 out.WriteBufferFixEndianness(&rangeImage(0, 0), rangeImage.size());
49 out.WriteAs<uint16_t>(intensityImage.cols());
50 out.WriteAs<uint16_t>(intensityImage.rows());
51 if (!intensityImage.empty())
52 out.WriteBufferFixEndianness(
53 &intensityImage(0, 0), intensityImage.size());
55 out.WriteAs<uint16_t>(rangeOtherLayers.size());
56 for (
const auto& ly : rangeOtherLayers)
61 out.WriteBufferFixEndianness(&ly.second(0, 0), ly.second.size());
71 in >> timestamp >> sensorLabel >> rowCount >> columnCount >>
72 rangeResolution >> startAzimuth >> azimuthSpan >>
73 sweepDuration >> lidarModel >> minRange >> maxRange >>
74 sensorPose >> originalReceivedTimestamp >>
75 has_satellite_timestamp;
77 const auto nCols = in.
ReadAs<uint16_t>(),
78 nRows = in.
ReadAs<uint16_t>();
79 rangeImage.resize(nRows, nCols);
80 if (!rangeImage.empty())
82 &rangeImage(0, 0), rangeImage.size());
85 const auto nIntCols = in.
ReadAs<uint16_t>(),
86 nIntRows = in.
ReadAs<uint16_t>();
87 intensityImage.resize(nIntRows, nIntCols);
88 if (!intensityImage.empty())
90 &intensityImage(0, 0), intensityImage.size());
93 const auto nOtherLayers = in.
ReadAs<uint16_t>();
94 rangeOtherLayers.clear();
95 for (
size_t i = 0; i < nOtherLayers; i++)
99 auto& im = rangeOtherLayers[name];
100 im.resize(nRows, nCols);
112 CObservation::getDescriptionAsText(o);
113 o <<
"Homogeneous matrix for the sensor 3D pose, relative to "
117 << sensorPose << endl;
119 o <<
"lidarModel: " << lidarModel <<
"\n";
120 o <<
"Range rows=" << rowCount <<
" cols=" << columnCount <<
"\n";
121 o <<
"Range resolution=" << rangeResolution <<
" [meter]\n";
124 o <<
"Sweep duration: " << sweepDuration <<
" [s]\n";
126 "Sensor min/max range: %.02f / %.02f m\n", minRange, maxRange);
127 o <<
"has_satellite_timestamp: " << (has_satellite_timestamp ?
"YES" :
"NO")
129 o <<
"originalReceivedTimestamp: "
139 using degree_cents = uint16_t;
140 using gps_microsecs = uint32_t;
159 gps_microsecs last_pkt_tim = std::numeric_limits<gps_microsecs>::max();
160 degree_cents last_pkt_az = 0;
163 std::map<degree_cents, gps_microsecs> azimuth2timestamp;
164 std::multiset<double> rotspeed;
169 rowCount = num_lasers;
175 const double timeBetweenLastTwoBlocks =
179 rangeImage.setZero(rowCount, columnCount);
180 intensityImage.setZero(rowCount, columnCount);
181 rangeOtherLayers.clear();
187 for (
size_t pktIdx = 0; pktIdx < o.
scan_packets.size(); pktIdx++)
191 model = pkt.velodyne_model_ID;
193 const degree_cents pkt_azimuth = pkt.blocks[0].rotation();
195 if (last_pkt_tim != std::numeric_limits<uint32_t>::max())
199 const double dT = 1e-6 * (pkt.gps_timestamp() - last_pkt_tim);
200 const auto dAzimuth = 1e-2 * (pkt_azimuth - last_pkt_az);
201 const auto estRotVel = dAzimuth / dT;
202 rotspeed.insert(estRotVel);
204 last_pkt_tim = pkt.gps_timestamp();
205 last_pkt_az = pkt_azimuth;
207 azimuth2timestamp[pkt_azimuth] = pkt.gps_timestamp();
214 const double maxRotSpeed = *rotspeed.rbegin(),
215 minRotSpeed = *rotspeed.begin();
217 ASSERT_BELOW_((maxRotSpeed - minRotSpeed) / maxRotSpeed, 0.01);
220 const double rotVel_degps = [&]() {
221 auto it = rotspeed.begin();
222 std::advance(it, rotspeed.size() / 2);
232 const double curAng =
234 const double nextAng =
239 azimuthSpan += incrAng;
245 const int dsr_offset =
247 const bool block_is_dual_strongest_range =
249 ((block & 0x01) != 0));
250 const bool block_is_dual_last_range =
252 ((block & 0x01) == 0));
256 if (!pkt.blocks[block]
261 const auto rawLaserId =
static_cast<uint8_t
>(dsr + dsr_offset);
262 uint8_t laserId = rawLaserId;
266 if (num_lasers == 16)
282 pkt.blocks[block].laser_returns[k].distance() +
283 static_cast<uint16_t
>(
286 const auto columnIdx = [&]() {
309 block_is_dual_strongest_range)
312 rangeImage(laserId, columnIdx) =
distance;
315 intensityImage(laserId, columnIdx) =
316 pkt.blocks[block].laser_returns[k].intensity();
318 else if (block_is_dual_last_range)
321 auto& r = rangeOtherLayers[
"STRONGEST"];
323 if (
static_cast<size_t>(r.rows()) != num_lasers)
324 r.setZero(rowCount, columnCount);
337 const auto microsecs_1st_pkt = o.
scan_packets.begin()->gps_timestamp();
338 const auto microsecs_last_pkt = o.
scan_packets.rbegin()->gps_timestamp();
339 sweepDuration = 1e-6 * (microsecs_last_pkt - microsecs_1st_pkt) +
340 timeBetweenLastTwoBlocks;
346 lidarModel =
"HDL-32E";
349 lidarModel =
"VLP-16";
352 lidarModel =
"Unknown";
367 this->has_satellite_timestamp =
false;
377 this->rangeImage.setZero(rowCount, columnCount);
378 this->intensityImage.setZero(rowCount, columnCount);
379 this->rangeOtherLayers.clear();
380 this->rangeResolution = 0.01;
386 uint16_t& range_out = rangeImage(0, i);
387 uint8_t& intensity_out = intensityImage(0, i);
393 const uint16_t r_discr =
394 static_cast<uint16_t
>((r /
d2f(rangeResolution)) + 0.5f);
403 this->lidarModel = std::string(
"2D_SCAN_") + this->sensorLabel;
432 fromPointCloud(*oPc);