Program Listing for File OS1_util.hpp
↰ Return to documentation for file (include/ros2_ouster/OS1/OS1_util.hpp
)
// Copyright 2020, Steve Macenski
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef ROS2_OUSTER__OS1__OS1_UTIL_HPP_
#define ROS2_OUSTER__OS1__OS1_UTIL_HPP_
#include <algorithm>
#include <functional>
#include <iterator>
#include <vector>
#include "ros2_ouster/OS1/OS1_packet.hpp"
namespace OS1
{
inline std::vector<double> make_xyz_lut(
int W, int H,
const std::vector<double> & azimuth_angles,
const std::vector<double> & altitude_angles)
{
const int n = W * H;
std::vector<double> xyz = std::vector<double>(3 * n, 0);
for (int icol = 0; icol < W; icol++) {
double h_angle_0 = 2.0 * M_PI * icol / W;
for (int ipx = 0; ipx < H; ipx++) {
int ind = 3 * (icol * H + ipx);
double h_angle =
(azimuth_angles.at(ipx) * 2 * M_PI / 360.0) + h_angle_0;
xyz[ind + 0] = std::cos(altitude_angles[ipx] * 2 * M_PI / 360.0) *
std::cos(h_angle);
xyz[ind + 1] = -std::cos(altitude_angles[ipx] * 2 * M_PI / 360.0) *
std::sin(h_angle);
xyz[ind + 2] = std::sin(altitude_angles[ipx] * 2 * M_PI / 360.0);
}
}
return xyz;
}
inline std::vector<int> get_px_offset(int lidar_mode)
{
auto repeat = [](int n, const std::vector<int> & v) {
std::vector<int> res{};
for (int i = 0; i < n; i++) {
res.insert(res.end(), v.begin(), v.end());
}
return res;
};
switch (lidar_mode) {
case 512:
return repeat(16, {0, 3, 6, 9});
case 1024:
return repeat(16, {0, 6, 12, 18});
case 2048:
return repeat(16, {0, 12, 24, 36});
default:
return std::vector<int>{64, 0};
}
}
template<typename iterator_type, typename F, typename C>
std::function<void(const uint8_t *, iterator_type it, uint64_t)> batch_to_iter(
const std::vector<double> & xyz_lut, int W, int H,
const typename iterator_type::value_type & empty, C && c, F && f)
{
int next_m_id{W};
int32_t cur_f_id{-1};
int64_t scan_ts{-1L};
return [ = ](const uint8_t * packet_buf, iterator_type it,
uint64_t override_ts) mutable {
for (int icol = 0; icol < OS1::columns_per_buffer; icol++) {
const uint8_t * col_buf = OS1::nth_col(icol, packet_buf);
const uint16_t m_id = OS1::col_measurement_id(col_buf);
const uint16_t f_id = OS1::col_frame_id(col_buf);
const uint64_t ts = OS1::col_timestamp(col_buf);
const bool valid = OS1::col_valid(col_buf) == 0xffffffff;
// drop invalid / out-of-bounds data in case of misconfiguration
if (!valid || m_id >= W || f_id + 1 == cur_f_id) {
continue;
}
if (f_id != cur_f_id) {
// if not initializing with first packet
if (scan_ts != -1) {
// zero out remaining missing columns
std::fill(it + (H * next_m_id), it + (H * W), empty);
f(override_ts == 0 ? scan_ts : override_ts);
}
// start new frame
scan_ts = ts;
next_m_id = 0;
cur_f_id = f_id;
}
// zero out missing columns if we jumped forward
if (m_id >= next_m_id) {
std::fill(it + (H * next_m_id), it + (H * m_id), empty);
next_m_id = m_id + 1;
}
// index of the first point in current packet
const int idx = H * m_id;
for (uint8_t ipx = 0; ipx < H; ipx++) {
const uint8_t * px_buf = OS1::nth_px(ipx, col_buf);
uint32_t r = OS1::px_range(px_buf);
int ind = 3 * (idx + ipx);
// x, y, z(m), intensity, ts, reflectivity, ring, column,
// noise, range (mm)
it[idx + ipx] = c(
r * 0.001f * xyz_lut[ind + 0],
r * 0.001f * xyz_lut[ind + 1],
r * 0.001f * xyz_lut[ind + 2],
OS1::px_signal_photons(px_buf), ts - scan_ts,
OS1::px_reflectivity(px_buf), ipx, m_id,
OS1::px_noise_photons(px_buf), r);
}
}
};
}
} // namespace OS1
#endif // ROS2_OUSTER__OS1__OS1_UTIL_HPP_