pyramid_map_node.h

Go to the documentation of this file.

/******************************************************************************
 * Copyright 2019 The Apollo Authors. All Rights Reserved.
 *
 * 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.
 *****************************************************************************/
#pragma once

#include <vector>

#include "Eigen/Core"

#include "modules/localization/msf/local_pyramid_map/base_map/base_map_node.h"
#include "modules/localization/msf/local_pyramid_map/pyramid_map/pyramid_map_matrix.h"

namespace apollo {
namespace localization {
namespace msf {
namespace pyramid_map {

class PyramidMapNode : public BaseMapNode {
 public:
  PyramidMapNode();
  ~PyramidMapNode();

 public:
  virtual void Init(const BaseMapConfig* map_config);
  virtual void Init(const BaseMapConfig* map_config, const MapNodeIndex& index,
                    bool create_map_cells = true);

  void BottomUpSafe();

  void BottomUpBase();

  bool AddValueIfInBound(const Eigen::Vector3d& coordinate,
                         unsigned char intensity, unsigned int level = 0);
  void AddValueIfInBound(const std::vector<Eigen::Vector3d>& coordinates,
                         const std::vector<unsigned char>& intensity,
                         unsigned int level = 0);
  bool GetCoordinate(const Eigen::Vector2d& coordinate, unsigned int level,
                     unsigned int* x, unsigned int* y) const;
  Eigen::Vector2d GetCoordinate(unsigned int level, unsigned int x,
                                unsigned int y) const;

  virtual bool GetCoordinate(const Eigen::Vector2d& coordinate, unsigned int* x,
                             unsigned int* y) const;
  virtual bool GetCoordinate(const Eigen::Vector3d& coordinate, unsigned int* x,
                             unsigned int* y) const;

  virtual Eigen::Vector2d GetCoordinate(unsigned int x, unsigned int y) const;

  float GetIntensitySafe(const Eigen::Vector3d& coordinate,
                         unsigned int level = 0) const;
  float GetIntensityVarSafe(const Eigen::Vector3d& coordinate,
                            unsigned int level = 0) const;
  float GetAltitudeSafe(const Eigen::Vector3d& coordinate,
                        unsigned int level = 0) const;
  float GetAltitudeVarSafe(const Eigen::Vector3d& coordinate,
                           unsigned int level = 0) const;
  float GetGroundAltitudeSafe(const Eigen::Vector3d& coordinate,
                              unsigned int level = 0) const;
  unsigned int GetCountSafe(const Eigen::Vector3d& coordinate,
                            unsigned int level = 0) const;
  unsigned int GetGroundCountSafe(const Eigen::Vector3d& coordinate,
                                  unsigned int level = 0) const;

  float GetIntensity(const Eigen::Vector3d& coordinate,
                     unsigned int level = 0) const;
  float GetIntensityVar(const Eigen::Vector3d& coordinate,
                        unsigned int level = 0) const;
  float GetAltitude(const Eigen::Vector3d& coordinate,
                    unsigned int level = 0) const;
  float GetAltitudeVar(const Eigen::Vector3d& coordinate,
                       unsigned int level = 0) const;
  float GetGroundAltitude(const Eigen::Vector3d& coordinate,
                          unsigned int level = 0) const;
  unsigned int GetCount(const Eigen::Vector3d& coordinate,
                        unsigned int level = 0) const;
  unsigned int GetGroundCount(const Eigen::Vector3d& coordinate,
                              unsigned int level = 0) const;

  double ComputeMeanIntensity(unsigned int level = 0);

 private:
  std::vector<float> resolutions_mr_;
};

}  // namespace pyramid_map
}  // namespace msf
}  // namespace localization
}  // namespace apollo