lossless_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 <vector>

#include "cyber/common/log.h"
#include "modules/localization/msf/local_map/base_map/base_map_matrix.h"
#include "modules/localization/msf/local_map/base_map/base_map_node.h"

namespace apollo {
namespace localization {
namespace msf {

#define IDL_CAR_NUM_RESERVED_MAP_LAYER 2U

struct LosslessMapSingleCell {
  LosslessMapSingleCell();
  inline void Reset();
  void AddSample(const float new_altitude, const float new_intensity);
  LosslessMapSingleCell& operator=(const LosslessMapSingleCell& ref);
  unsigned int LoadBinary(unsigned char* buf);
  unsigned int CreateBinary(unsigned char* buf, unsigned int buf_size) const;
  unsigned int GetBinarySize() const;

  float intensity;
  float intensity_var;
  float altitude;
  float altitude_var;
  unsigned int count;
};

struct LosslessMapCell {
  LosslessMapCell();
  void Reset();
  void SetValueLayer(double altitude, unsigned char intensity,
                     double altitude_thres = 10.0);
  void SetValue(double altitude, unsigned char intensity);
  unsigned int LoadBinary(unsigned char* buf);
  unsigned int CreateBinary(unsigned char* buf, unsigned int buf_size) const;
  unsigned int GetBinarySize() const;

  unsigned int GetLayerId(double altitude) const;
  void GetValue(std::vector<unsigned char>* values) const;
  void GetVar(std::vector<float>* vars) const;
  void GetAlt(std::vector<float>* alts) const;
  void GetAltVar(std::vector<float>* alt_vars) const;
  void GetCount(std::vector<unsigned int>* counts) const;
  inline unsigned char GetValue() const {
    return static_cast<unsigned char>(map_cells[0].intensity);
  }
  inline float GetVar() const { return map_cells[0].intensity_var; }
  inline float GetAlt() const { return map_cells[0].altitude; }
  inline float GetAltVar() const { return map_cells[0].altitude_var; }
  inline unsigned int GetCount() const { return map_cells[0].count; }
  LosslessMapSingleCell& GetLayer(unsigned int layer_id) {
    DCHECK_LT(layer_id, layer_num);
    return map_cells[layer_id];
  }
  const LosslessMapSingleCell& GetLayer(unsigned int layer_id) const {
    DCHECK_LT(layer_id, layer_num);
    return map_cells[layer_id];
  }

  unsigned int layer_num;
  LosslessMapSingleCell map_cells[IDL_CAR_NUM_RESERVED_MAP_LAYER];
};

class LosslessMapMatrix : public BaseMapMatrix {
 public:
  LosslessMapMatrix();
  ~LosslessMapMatrix();
  LosslessMapMatrix(const LosslessMapMatrix& matrix);

  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 LosslessMapCell& GetMapCell(unsigned int row,
                                           unsigned int col) const {
    DCHECK_LT(row, rows_);
    DCHECK_LT(col, cols_);
    return map_cells_[row * cols_ + col];
  }
  inline LosslessMapCell& GetMapCell(unsigned int row, unsigned int col) {
    DCHECK_LT(row, rows_);
    DCHECK_LT(col, cols_);
    return map_cells_[row * cols_ + col];
  }

  inline LosslessMapCell* operator[](int row) {
    return map_cells_ + row * cols_;
  }
  inline const LosslessMapCell* operator[](int row) const {
    return map_cells_ + row * cols_;
  }

 protected:
  unsigned int rows_;
  unsigned int cols_;
  LosslessMapCell* map_cells_;
};

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