.. _program_listing_file_include_ros2_ouster_OS1_OS1_util.hpp:

Program Listing for File OS1_util.hpp
=====================================

|exhale_lsh| :ref:`Return to documentation for file <file_include_ros2_ouster_OS1_OS1_util.hpp>` (``include/ros2_ouster/OS1/OS1_util.hpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   // 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_