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_