ndt_map_matrix.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 <limits>
#include <memory>
#include <unordered_map>
#include <vector>

#include "Eigen/Eigenvalues"

#include "modules/localization/msf/local_pyramid_map/base_map/base_map_config.h"
#include "modules/localization/msf/local_pyramid_map/base_map/base_map_matrix.h"

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

class NdtMapSingleCell {
 public:
  NdtMapSingleCell();
  void Reset();
  size_t LoadBinary(const unsigned char* buf);
  size_t CreateBinary(unsigned char* buf, size_t buf_size) const;
  size_t GetBinarySize() const;
  NdtMapSingleCell& operator=(const NdtMapSingleCell& ref);

  static void Reduce(NdtMapSingleCell* cell, const NdtMapSingleCell& cell_new);

  void AddSample(const float intensity, const float altitude,
                 const Eigen::Vector3f& centroid, bool is_road = false);

  void MergeCell(const float intensity, const float intensity_var,
                 const unsigned int road_pt_count, const unsigned int count,
                 const Eigen::Vector3f& centroid,
                 const Eigen::Matrix3f& centroid_cov);

  void MergeCell(const NdtMapSingleCell& cell_new);

  void CentroidEigenSolver(const Eigen::Matrix3f& centroid_cov);

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 public:
  float intensity_ = 0;
  float intensity_var_ = 0;
  unsigned int road_pt_count_ = 0;
  unsigned int count_ = 0;

  Eigen::Vector3f centroid_;
  Eigen::Matrix3f centroid_average_cov_;
  Eigen::Matrix3f centroid_icov_;
  unsigned char is_icov_available_ = 0;
  const unsigned int minimum_points_threshold_ = 6;
};

class NdtMapCells {
 public:
  NdtMapCells();
  void Reset();
  int AddSample(const float intensity, const float altitude,
                const float resolution, const Eigen::Vector3f& centroid,
                bool is_road = false);

  size_t LoadBinary(const unsigned char* buf);
  size_t CreateBinary(unsigned char* buf, size_t buf_size) const;
  size_t GetBinarySize() const;

  static int CalAltitudeIndex(const float resolution, const float altitude);

  static float CalAltitude(const float resolution, const int altitude_index);

  static void Reduce(NdtMapCells* cell, const NdtMapCells& cell_new);

 public:
  std::unordered_map<int, NdtMapSingleCell> cells_;
  int max_altitude_index_;
  int min_altitude_index_;
  std::vector<int> road_cell_indices_;
};

class NdtMapMatrix : public BaseMapMatrix {
 public:
  NdtMapMatrix();
  ~NdtMapMatrix();
  NdtMapMatrix(const NdtMapMatrix& cells);

  virtual void Init(const BaseMapConfig& config);
  virtual void Reset();

  void Init(unsigned int rows, unsigned int cols);
  void Reset(unsigned int rows, unsigned int cols);
  virtual size_t LoadBinary(const unsigned char* buf);
  virtual size_t CreateBinary(unsigned char* buf, size_t buf_size) const;
  virtual size_t GetBinarySize() const;

  virtual bool GetIntensityImg(cv::Mat* intensity_img) const;

  inline const NdtMapCells& GetMapCell(unsigned int row,
                                       unsigned int col) const {
    assert(row < rows_);
    assert(col < cols_);
    return map3d_cells_[row * cols_ + col];
  }
  inline NdtMapCells& GetMapCell(unsigned int row, unsigned int col) {
    assert(row < rows_);
    assert(col < cols_);
    return map3d_cells_[row * cols_ + col];
  }
  unsigned int GetRows() const { return rows_; }
  unsigned int GetCols() const { return cols_; }

  static void Reduce(NdtMapMatrix* cells, const NdtMapMatrix& cells_new);

 private:
  unsigned int rows_;
  unsigned int cols_;
  std::unique_ptr<NdtMapCells[]> map3d_cells_;
};

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