ndt_map_matrix.h

Go to the documentation of this file.

/******************************************************************************
 * Copyright 2018 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_map/base_map/base_map_config.h"
#include "modules/localization/msf/local_map/base_map/base_map_matrix.h"

namespace apollo {
namespace localization {
namespace msf {

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

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

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

  inline 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);

  inline void MergeCell(const NdtMapSingleCell& cell_new);

  inline 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);

  unsigned int LoadBinary(unsigned char* buf);
  unsigned int CreateBinary(unsigned char* buf, unsigned int buf_size) const;
  unsigned int 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(const BaseMapConfig* config);

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

  virtual void 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_;
};

inline void NdtMapSingleCell::Reset() {
  intensity_ = 0.0;
  intensity_var_ = 0.0;
  road_pt_count_ = 0;
  count_ = 0;
  centroid_ = Eigen::Vector3f::Zero();
  centroid_average_cov_ = Eigen::Matrix3f::Zero();
  centroid_icov_ = Eigen::Matrix3f::Identity();
  is_icov_available_ = false;
}

inline void NdtMapSingleCell::AddSample(const float intensity,
                                        const float altitude,
                                        const Eigen::Vector3f centroid,
                                        bool is_road) {
  ++count_;
  float v1 = intensity - intensity_;
  intensity_ += v1 / static_cast<float>(count_);
  float v2 = intensity - intensity_;
  intensity_var_ = (static_cast<float>(count_ - 1) * intensity_var_ + v1 * v2) /
                   static_cast<float>(count_);

  if (is_road) {
    ++road_pt_count_;
  }

  Eigen::Vector3f v3 = centroid - centroid_;
  centroid_ += v3 / static_cast<float>(count_);
  Eigen::Matrix3f v4 = centroid * centroid.transpose() - centroid_average_cov_;
  centroid_average_cov_ += v4 / static_cast<float>(count_);
  CentroidEigenSolver(centroid_average_cov_);
}

inline void NdtMapSingleCell::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) {
  unsigned int new_count = count_ + count;
  float p0 = static_cast<float>(count_) / static_cast<float>(new_count);
  float p1 = static_cast<float>(count) / static_cast<float>(new_count);
  float intensity_diff = intensity_ - intensity;

  intensity_ = intensity_ * p0 + intensity * p1;
  intensity_var_ = intensity_var_ * p0 + intensity_var * p1 +
                   intensity_diff * intensity_diff * p0 * p1;

  centroid_[0] = centroid_[0] * p0 + centroid[0] * p1;
  centroid_[1] = centroid_[1] * p0 + centroid[1] * p1;
  centroid_[2] = centroid_[2] * p0 + centroid[2] * p1;

  count_ = new_count;
  road_pt_count_ += road_pt_count;
}

inline void NdtMapSingleCell::MergeCell(const NdtMapSingleCell& cell_new) {
  MergeCell(cell_new.intensity_, cell_new.intensity_var_,
            cell_new.road_pt_count_, cell_new.count_, cell_new.centroid_,
            cell_new.centroid_average_cov_);
}

inline void NdtMapSingleCell::CentroidEigenSolver(
    const Eigen::Matrix3f& centroid_cov) {
  // Contain more than five points, we calculate the eigen vector/value of cov.
  // [Magnusson 2009]
  if (count_ >= minimum_points_threshold_) {
    Eigen::Matrix3f cov = centroid_cov - centroid_ * centroid_.transpose();
    cov *= static_cast<float>((count_ - 1.0) / count_);
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver;
    eigen_solver.compute(cov);
    Eigen::Matrix3f eigen_val = eigen_solver.eigenvalues().asDiagonal();
    Eigen::Matrix3f centroid_evecs = eigen_solver.eigenvectors();
    if (eigen_val(0, 0) < 0 || eigen_val(1, 1) < 0 || eigen_val(2, 2) <= 0) {
      is_icov_available_ = 0;
      return;
    }
    // Avoid matrices near singularities (eq 6.11) [Magnusson 2009].
    float min_covar_eigvalue = 0.01f * eigen_val(2, 2);
    if (eigen_val(0, 0) < min_covar_eigvalue) {
      eigen_val(0, 0) = min_covar_eigvalue;
      if (eigen_val(1, 1) < min_covar_eigvalue) {
        eigen_val(1, 1) = min_covar_eigvalue;
      }
    }
    // Calculate inverse covariance
    centroid_icov_ =
        (centroid_evecs * eigen_val * centroid_evecs.inverse()).inverse();
    if (centroid_icov_.maxCoeff() == std::numeric_limits<float>::infinity() ||
        centroid_icov_.minCoeff() == -std::numeric_limits<float>::infinity()) {
      is_icov_available_ = 0;
      return;
    }
    // Set icov available
    is_icov_available_ = 1;
  }
}

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