LCOV - code coverage report
Current view: top level - src/common/autoware_auto_geometry/include/geometry - lookup_table.hpp (source / functions) Hit Total Coverage
Test: lcov.total.filtered Lines: 39 41 95.1 %
Date: 2021-01-26 05:02:50 Functions: 25 25 100.0 %
Legend: Lines: hit not hit | Branches: + taken - not taken # not executed Branches: 25 38 65.8 %

           Branch data     Line data    Source code
       1                 :            : // Copyright 2017-2020 the Autoware Foundation
       2                 :            : //
       3                 :            : // Licensed under the Apache License, Version 2.0 (the "License");
       4                 :            : // you may not use this file except in compliance with the License.
       5                 :            : // You may obtain a copy of the License at
       6                 :            : //
       7                 :            : //     http://www.apache.org/licenses/LICENSE-2.0
       8                 :            : //
       9                 :            : // Unless required by applicable law or agreed to in writing, software
      10                 :            : // distributed under the License is distributed on an "AS IS" BASIS,
      11                 :            : // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
      12                 :            : // See the License for the specific language governing permissions and
      13                 :            : // limitations under the License.
      14                 :            : //
      15                 :            : // Co-developed by Tier IV, Inc. and Apex.AI, Inc.
      16                 :            : 
      17                 :            : /// \file
      18                 :            : /// \brief This file contains a 1D linear lookup table implementation
      19                 :            : 
      20                 :            : #ifndef GEOMETRY__LOOKUP_TABLE_HPP_
      21                 :            : #define GEOMETRY__LOOKUP_TABLE_HPP_
      22                 :            : 
      23                 :            : #include <cmath>
      24                 :            : #include <stdexcept>
      25                 :            : #include <vector>
      26                 :            : 
      27                 :            : #include "common/types.hpp"
      28                 :            : #include "geometry/interval.hpp"
      29                 :            : 
      30                 :            : namespace autoware
      31                 :            : {
      32                 :            : namespace common
      33                 :            : {
      34                 :            : namespace helper_functions
      35                 :            : {
      36                 :            : 
      37                 :            : namespace
      38                 :            : {
      39                 :            : 
      40                 :            : /**
      41                 :            :  * @brief Compute a scaling between 'a' and 'b'
      42                 :            :  * @note scaling is clamped to be in the interval [0, 1]
      43                 :            :  */
      44                 :            : template<typename T, typename U>
      45                 :         18 : T interpolate(const T & a, const T & b, U scaling)
      46                 :            : {
      47   [ +  +  +  -  :         18 :   static const geometry::Interval<U> UNIT_INTERVAL(static_cast<U>(0), static_cast<U>(1));
             +  -  -  - ]
      48                 :         18 :   scaling = geometry::Interval<U>::clamp_to(UNIT_INTERVAL, scaling);
      49                 :            : 
      50                 :         18 :   const auto m = (b - a);
      51                 :         18 :   const auto offset = static_cast<U>(m) * scaling;
      52                 :         18 :   return a + static_cast<T>(offset);
      53                 :            : }
      54                 :            : 
      55                 :            : // TODO(c.ho) support more forms of interpolation as template functor
      56                 :            : // Actual lookup logic, assuming all invariants hold:
      57                 :            : // Throw if value is not finite
      58                 :            : template<typename T>
      59                 :         16 : T lookup_impl_1d(const std::vector<T> & domain, const std::vector<T> & range, const T value)
      60                 :            : {
      61         [ -  + ]:         16 :   if (!std::isfinite(value)) {
      62         [ #  # ]:          0 :     throw std::domain_error{"Query value is not finite (NAN or INF)"};
      63                 :            :   }
      64         [ +  + ]:         16 :   if (value <= domain.front()) {
      65                 :          4 :     return range.front();
      66         [ +  + ]:         12 :   } else if (value >= domain.back()) {
      67                 :          4 :     return range.back();
      68                 :            :   } else {
      69                 :            :     // Fall through to normal case
      70                 :            :   }
      71                 :            : 
      72                 :          8 :   auto second_idx{0U};
      73         [ +  - ]:         12 :   for (auto idx = 1U; idx < domain.size(); ++idx) {
      74         [ +  + ]:         12 :     if (value < domain[idx]) {
      75                 :          8 :       second_idx = idx;
      76                 :          8 :       break;
      77                 :            :     }
      78                 :            :   }
      79                 :            :   // T must be a floating point between 0 and 1
      80                 :          8 :   const auto num = static_cast<double>(value - domain[second_idx - 1U]);
      81                 :          8 :   const auto den = static_cast<double>(domain[second_idx] - domain[second_idx - 1U]);
      82                 :          8 :   const auto t = num / den;
      83                 :          8 :   const auto val = interpolate(range[second_idx - 1U], range[second_idx], t);
      84                 :          8 :   return static_cast<T>(val);
      85                 :            : }
      86                 :            : 
      87                 :            : // Check invariants for table lookup:
      88                 :            : template<typename T>
      89                 :         28 : void check_table_lookup_invariants(const std::vector<T> & domain, const std::vector<T> & range)
      90                 :            : {
      91         [ +  + ]:         28 :   if (domain.size() != range.size()) {
      92         [ +  - ]:          4 :     throw std::domain_error{"Domain's size does not match range's"};
      93                 :            :   }
      94         [ +  + ]:         24 :   if (domain.empty()) {
      95         [ +  - ]:          4 :     throw std::domain_error{"Empty domain or range"};
      96                 :            :   }
      97                 :            :   // Check if sorted: Can start at 1 because not empty
      98         [ +  + ]:         52 :   for (auto idx = 1U; idx < domain.size(); ++idx) {
      99         [ +  + ]:         36 :     if (domain[idx] <= domain[idx - 1U]) {  // This is safe because indexing starts at 1
     100         [ +  - ]:          4 :       throw std::domain_error{"Domain is not sorted"};
     101                 :            :     }
     102                 :            :   }
     103                 :         16 : }
     104                 :            : }  // namespace
     105                 :            : 
     106                 :            : /// Do a 1D table lookup: Does some semi-expensive O(N) error checking first.
     107                 :            : /// If query value fall out of the domain, then the value at the corresponding edge of the domain is
     108                 :            : /// returned.
     109                 :            : /// \param[in] domain The domain, or set of x values
     110                 :            : /// \param[in] range The range, or set of y values
     111                 :            : /// \param[in] value The point in the domain to query, x
     112                 :            : /// \return A linearly interpolated value y, corresponding to the query, x
     113                 :            : /// \throw std::domain_error If domain or range is empty
     114                 :            : /// \throw std::domain_error If range is not the same size as domain
     115                 :            : /// \throw std::domain_error If domain is not sorted
     116                 :            : /// \throw std::domain_error If value is not finite (NAN or INF)
     117                 :            : /// \tparam T The type of the function, must be interpolatable
     118                 :            : template<typename T>
     119                 :         12 : T lookup_1d(const std::vector<T> & domain, const std::vector<T> & range, const T value)
     120                 :            : {
     121                 :         12 :   check_table_lookup_invariants(domain, range);
     122                 :            : 
     123                 :          0 :   return lookup_impl_1d(domain, range, value);
     124                 :            : }
     125                 :            : 
     126                 :            : /// A class wrapping a 1D lookup table. Intended for more frequent lookups. Error checking is pushed
     127                 :            : /// into the constructor and not done in the lookup function call
     128                 :            : /// \tparam T The type of the function, must be interpolatable
     129                 :            : template<typename T>
     130                 :            : class LookupTable1D
     131                 :            : {
     132                 :            : public:
     133                 :            :   /// Constructor
     134                 :            :   /// \param[in] domain The domain, or set of x values
     135                 :            :   /// \param[in] range The range, or set of y values
     136                 :            :   /// \throw std::domain_error If domain or range is empty
     137                 :            :   /// \throw std::domain_error If range is not the same size as domain
     138                 :            :   /// \throw std::domain_error If domain is not sorted
     139                 :         16 :   LookupTable1D(const std::vector<T> & domain, const std::vector<T> & range)
     140                 :            :   : m_domain{domain},
     141         [ +  - ]:         16 :     m_range{range}
     142                 :            :   {
     143         [ +  - ]:         16 :     check_table_lookup_invariants(m_domain, m_range);
     144                 :         16 :   }
     145                 :            : 
     146                 :            :   /// Move constructor
     147                 :            :   /// \param[in] domain The domain, or set of x values
     148                 :            :   /// \param[in] range The range, or set of y values
     149                 :            :   /// \throw std::domain_error If domain or range is empty
     150                 :            :   /// \throw std::domain_error If range is not the same size as domain
     151                 :            :   /// \throw std::domain_error If domain is not sorted
     152                 :            :   LookupTable1D(std::vector<T> && domain, std::vector<T> && range)
     153                 :            :   : m_domain{domain},
     154                 :            :     m_range{range}
     155                 :            :   {
     156                 :            :     check_table_lookup_invariants(m_domain, m_range);
     157                 :            :   }
     158                 :            : 
     159                 :            :   /// Do a 1D table lookup
     160                 :            :   /// If query value fall out of the domain, then the value at the corresponding edge of the domain
     161                 :            :   /// is returned.
     162                 :            :   /// \param[in] value The point in the domain to query, x
     163                 :            :   /// \return A linearly interpolated value y, corresponding to the query, x
     164                 :            :   /// \throw std::domain_error If value is not finite
     165                 :         16 :   T lookup(const T value) const
     166                 :            :   {
     167                 :         16 :     return lookup_impl_1d(m_domain, m_range, value);
     168                 :            :   }
     169                 :            :   /// Get the domain table
     170                 :            :   const std::vector<T> & domain() const noexcept {return m_domain;}
     171                 :            :   /// Get the range table
     172                 :            :   const std::vector<T> & range() const noexcept {return m_range;}
     173                 :            : 
     174                 :            : private:
     175                 :            :   std::vector<T> m_domain;
     176                 :            :   std::vector<T> m_range;
     177                 :            : };  // class LookupTable1D
     178                 :            : 
     179                 :            : }  // namespace helper_functions
     180                 :            : }  // namespace common
     181                 :            : }  // namespace autoware
     182                 :            : 
     183                 :            : 
     184                 :            : #endif  // GEOMETRY__LOOKUP_TABLE_HPP_

Generated by: LCOV version 1.14