TimeVaryingVolumetricGrid.hh

Go to the documentation of this file.

/*
 * Copyright (C) 2022 Open Source Robotics Foundation
 *
 * 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 GZ_MATH_TIME_VARYING_VOLUMETRIC_GRID_HH_
#define GZ_MATH_TIME_VARYING_VOLUMETRIC_GRID_HH_
 
#include <gz/math/TimeVaryingVolumetricGridLookupField.hh>
#include <gz/math/Vector3.hh>
 
#include <map>
#include <utility>
#include <vector>
 
namespace gz::math
{
template<typename T, typename V, typename S, typename P>
class TimeVaryingVolumetricGrid
{
  public: S CreateSession() const;
 
  public: S CreateSession(const T &_time) const;
 
  public: bool IsValid(const S &_session) const;
 
  public: std::optional<S> StepTo(const S &_session, const T &_time) const;
 
  public: std::optional<V> LookUp(const S &_session, const Vector3<P> &_pos)
    const;
 
  public: std::pair<Vector3<V>, Vector3<V>> Bounds(const S &_session) const;
};
 
template<typename T, typename V, typename P>
class TimeVaryingVolumetricGrid<T, V, InMemorySession<T, P>, P>
{
  public: InMemorySession<T, V> CreateSession() const
  {
    return indices.CreateSession();
  }
 
  public: InMemorySession<T, V> CreateSession(const T &_time) const
  {
    return indices.CreateSession(_time);
  }
 
  public: bool IsValid(const InMemorySession<T, P> &_session) const
  {
    return indices.IsValid(_session);
  }
 
  public: std::optional<InMemorySession<T, P>>
    StepTo(const InMemorySession<T, double> &_session, const T &_time) const
  {
    return indices.StepTo(_session, _time);
  }
 
  public: std::optional<V>
    LookUp(const InMemorySession<T, P> &_session,
      const Vector3<P> &_pos,
      const Vector3<V> &_tol = Vector3<V>{1e-6, 1e-6, 1e-6})
    const
  {
    auto points = indices.LookUp(_session, _pos, _tol);
    std::optional<V> result = indices.EstimateQuadrilinear(
      _session,
      points,
      values,
      values,
      _pos);
    return result;
  }
 
  public: std::pair<Vector3<V>, Vector3<V>> Bounds(
    const InMemorySession<T, P> &_session) const
  {
    return indices.Bounds(_session);
  }
 
  private: std::vector<V> values;
 
  private: TimeVaryingVolumetricGridLookupField
    <T, V, InMemorySession<T, P>> indices;
 
  template<typename U, typename S, typename X>
  friend class InMemoryTimeVaryingVolumetricGridFactory;
};
 
template<typename T, typename V = T, typename P = T>
using InMemoryTimeVaryingVolumetricGrid =
  TimeVaryingVolumetricGrid<T, V, InMemorySession<T, P>, P>;
 
template<typename T, typename V, typename P = double>
class InMemoryTimeVaryingVolumetricGridFactory
{
  public: void AddPoint(
    const T &_time, const Vector3<P> &_position, const V &_value)
  {
    _points[_time].emplace_back(_position, _value);
  }
 
  public: InMemoryTimeVaryingVolumetricGrid<T, V, P> Build() const
  {
    InMemoryTimeVaryingVolumetricGrid<T, V, P> grid;
    for (auto &[time, pts] : _points)
    {
      std::vector<Vector3d> cloud;
      std::vector<std::size_t> indices;
      for (auto &[pt, val] : pts)
      {
        grid.values.push_back(val);
        cloud.push_back(pt);
        indices.push_back(grid.values.size() - 1);
      }
      VolumetricGridLookupField<V> field(cloud, indices);
      grid.indices.AddVolumetricGridField(time, field);
    }
    return grid;
  }
 
  private: std::map<T, std::vector<std::pair<Vector3d, V>>> _points;
};
}  // namespace gz::math
#endif  // GZ_MATH_TIME_VARYING_VOLUMETRIC_GRID_HH_