Sematic-Cartographer/cartographer-master/cartographer/mapping/2d/probability_grid.cc

/*
 * Copyright 2016 The Cartographer Authors
 *
 * 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.
 */
#include "cartographer/mapping/2d/probability_grid.h"

#include <limits>
#include <iostream>
#include "absl/memory/memory.h"
#include "cartographer/mapping/probability_values.h"
#include "cartographer/mapping/submaps.h"
#include <queue>

namespace cartographer {
namespace mapping {

/**
 * @brief ProbabilityGrid的构造函数
 * 
 * @param[in] limits 地图坐标信息
 * @param[in] conversion_tables 转换表
 */
ProbabilityGrid::ProbabilityGrid(const MapLimits& limits,
                                 ValueConversionTables* conversion_tables)
    : Grid2D(limits, kMinCorrespondenceCost, kMaxCorrespondenceCost,
             conversion_tables),
      conversion_tables_(conversion_tables) {
}

ProbabilityGrid::ProbabilityGrid(const proto::Grid2D& proto,
                                 ValueConversionTables* conversion_tables)
    : Grid2D(proto, conversion_tables), conversion_tables_(conversion_tables) {
  CHECK(proto.has_probability_grid_2d());
  
}

// Sets the probability of the cell at 'cell_index' to the given
// 'probability'. Only allowed if the cell was unknown before.
// 将 索引 处单元格的概率设置为给定的概率, 仅当单元格之前处于未知状态时才允许
void ProbabilityGrid::SetProbability(const Eigen::Array2i& cell_index,
                                     const float probability) {
  // 获取对应栅格的引用
  uint16& cell =
      (*mutable_correspondence_cost_cells())[ToFlatIndex(cell_index)];
  CHECK_EQ(cell, kUnknownProbabilityValue);
  // 为栅格赋值 value
  cell =
      CorrespondenceCostToValue(ProbabilityToCorrespondenceCost(probability));
  // 更新bounding_box
  mutable_known_cells_box()->extend(cell_index.matrix());
}

// Applies the 'odds' specified when calling ComputeLookupTableToApplyOdds()
// to the probability of the cell at 'cell_index' if the cell has not already
// been updated. Multiple updates of the same cell will be ignored until
// FinishUpdate() is called. Returns true if the cell was updated.
// 如果单元格尚未更新,则将调用 ComputeLookupTableToApplyOdds() 时指定的 'odds' 应用于单元格在 'cell_index' 处的概率
// 在调用 FinishUpdate() 之前，将忽略同一单元格的多次更新。如果单元格已更新，则返回 true
//
// If this is the first call to ApplyOdds() for the specified cell, its value
// will be set to probability corresponding to 'odds'.
// 如果这是对指定单元格第一次调用 ApplyOdds(),则其值将设置为与 'odds' 对应的概率

// 使用查找表对指定栅格进行栅格值的更新

//。。新增函数
bool ProbabilityGrid::ApplyLookupTableHit(const Eigen::Array2i& cell_index,
                                       const std::vector<uint16>& table,
                                        const uint8 intensity) {
  DCHECK_EQ(table.size(), kUpdateMarker);
  const int flat_index = ToFlatIndex(cell_index);
  // 获取对应栅格的指针
  uint16* cell = &(*mutable_correspondence_cost_cells())[flat_index];
  // 对处于更新状态的栅格, 不再进行更新了
  if (*cell >= kUpdateMarker) {
    return false;
  }
  // 标记这个索引的栅格已经被更新过
  mutable_update_indices()->push_back(flat_index);

  if(((*mutable_correspondence_cost_cells2()).size())<flat_index) (*mutable_correspondence_cost_cells2()).resize((*mutable_correspondence_cost_cells()).size());
  //if(intensities!=0)
  ((*mutable_correspondence_cost_cells2())[flat_index]).push_back(intensity);

  // 更新栅格值
  *cell = table[*cell];
  DCHECK_GE(*cell, kUpdateMarker);
  // 更新bounding_box
  mutable_known_cells_box()->extend(cell_index.matrix());
  // jzq
  mutable_semantics_observer()->at(flat_index) = 1;
  ApplyLookupTableSemHit(intensity, flat_index);
  return true;
}

// jzq 每一个栅格使用一个vector来记录语义，固定其大小为7来保持语义的动态性
void ProbabilityGrid::ApplyLookupTableSemHit(const uint8 intensity, const int flat_index) {
  // 非视野内、已更新过语义 则不再更新语义
  if(intensity == 0 || mutable_semantics_update_indices()->at(flat_index))
    return ;

  std::vector<uint16>* counter = &(mutable_semantics_counter()->at(flat_index));
  counter->push_back(intensity);
  while(counter->size() > 7)
    counter->erase(counter->begin());

	std::map<int, int> tempCounter{};
	for (uint16 x : *counter)
	{
    if (tempCounter.find(x) == tempCounter.end())
      tempCounter[x] = 1;
    else
      tempCounter[x] += 1;
    if (tempCounter[x] > 3) {
      // 如果是新的语义，则直接更新
      if(x > 2) {
        mutable_semantics()->at(flat_index) = x;
        mutable_semantics_update_indices()->at(flat_index) = 1;
        return ;
      }
      // 如果是没有语义，则对于小物体允许更新语义，大物体则需要counter中全为1才允许更新
      // if(smallObjects.find(mutable_semantics()->at(flat_index)) != smallObjects.end()) {
      //   mutable_semantics()->at(flat_index) = x;
      //   mutable_semantics_update_indices()->at(flat_index) = 1;
      // }
      // 要求语义vector中是7个1才允许语义消失
      else if(tempCounter[x] > 6) {
        mutable_semantics()->at(flat_index) = x;
        mutable_semantics_update_indices()->at(flat_index) = 1;
      }
    }
  }
}

// jzq miss语义更新策略
void ProbabilityGrid::ApplyLookupTableSemMiss(const uint8 intensity, const int flat_index) {
  // jzq intensity>0表示摄像头视域，为0则表示其他角度雷达返回的点
  // 非摄像头视域，已更新过语义则不再更新
  if(intensity == 0 || mutable_semantics_update_indices()->at(flat_index))
    return ;
  // 测距范围外的小物体语义不更新
  if((smallObjects.find(semantics().at(flat_index)) != smallObjects.end()) && (!scanSemantics().at(flat_index)))
      return ;

  std::vector<uint16>* counter = &(mutable_semantics_counter()->at(flat_index));
  counter->push_back(1);
  
  while(counter->size() > 7)
    counter->erase(counter->begin());

  std::map<int, int> tempCounter{};
  for (uint16 x : *counter)
  {
    if (tempCounter.find(x) == tempCounter.end())
      tempCounter[x] = 1;
    else
      tempCounter[x] += 1;
    if (tempCounter[x] > 3) {
    if(x > 2) {
      mutable_semantics()->at(flat_index) = x;
      mutable_semantics_update_indices()->at(flat_index) = 1;
      return ;
    }
    // if(smallObjects.find(mutable_semantics()->at(flat_index)) != smallObjects.end()) {
    //   mutable_semantics()->at(flat_index) = x;
    //   mutable_semantics_update_indices()->at(flat_index) = 1;
    // }
    // 要求语义vector中是7个1才允许语义消失
    else if(tempCounter[x] > 6) {
      mutable_semantics()->at(flat_index) = x;
      mutable_semantics_update_indices()->at(flat_index) = 1;
    }
    }
  }
}

bool ProbabilityGrid::ApplyLookupTableMiss(const Eigen::Array2i& cell_index,
                                       const std::vector<uint16>& table,
                                       const uint8 intensity) {
  DCHECK_EQ(table.size(), kUpdateMarker);
  const int flat_index = ToFlatIndex(cell_index);
  // 获取对应栅格的指针
  uint16* cell = &(*mutable_correspondence_cost_cells())[flat_index];
  // 对处于更新状态的栅格, 不再进行更新了
  if (*cell >= kUpdateMarker) {
    return false;
  }
  // 标记这个索引的栅格已经被更新过
  mutable_update_indices()->push_back(flat_index);
  // 更新栅格值
  //。。。cell2_>append(range.hit.color)
  //*cell2 = table[*cell];
  *cell = table[*cell];
  DCHECK_GE(*cell, kUpdateMarker);
  // 更新bounding_box
  mutable_known_cells_box()->extend(cell_index.matrix());
  // jzq
  ApplyLookupTableSemMiss(intensity, flat_index);
  return true;
}

bool ProbabilityGrid::ApplyLookupTable(const Eigen::Array2i& cell_index,
                                       const std::vector<uint16>& table) {
  DCHECK_EQ(table.size(), kUpdateMarker);
  const int flat_index = ToFlatIndex(cell_index);
  // 获取对应栅格的指针
  uint16* cell = &(*mutable_correspondence_cost_cells())[flat_index];
  // 对处于更新状态的栅格, 不再进行更新了
  if (*cell >= kUpdateMarker) {
    return false;
  }
  // 标记这个索引的栅格已经被更新过
  mutable_update_indices()->push_back(flat_index);
  // 更新栅格值
  *cell = table[*cell];
  DCHECK_GE(*cell, kUpdateMarker);
  // 更新bounding_box
  mutable_known_cells_box()->extend(cell_index.matrix());
  return true;
}

GridType ProbabilityGrid::GetGridType() const {
  return GridType::PROBABILITY_GRID;
}

// Returns the probability of the cell with 'cell_index'.
// 获取 索引 处单元格的占用概率
float ProbabilityGrid::GetProbability(const Eigen::Array2i& cell_index) const {
  if (!limits().Contains(cell_index)) return kMinProbability;
  return CorrespondenceCostToProbability(ValueToCorrespondenceCost(
      correspondence_cost_cells()[ToFlatIndex(cell_index)]));
}
//int ColorGrid::GetColor(const Eigen::Array2i& cell_index) const  取出改结构第二维的众数 记得声明
//。。。新增函数getcolor
float ProbabilityGrid::GetColor(const Eigen::Array2i& cell_index) const {
  if (!limits().Contains(cell_index)) return 0;    //?？？
  // jzq 返回最近时刻赋给栅格的语义
  return semantics().at(ToFlatIndex(cell_index));
}

uint8 ProbabilityGrid::GetSemantics_observer(const Eigen::Array2i& cell_index) const {
  if (!limits().Contains(cell_index)) return 0;    //?？？
  // jzq 返回子图被摄像头观测过的区域
  // 本来想返回scanSemantics_observer_对应的值，但其视野范围太窄，更新好像有问题
  // 可考虑同时结合scanSemantics_observer_的信息解决子图融合时的小物体语义问题
  // return scanSemantics().at(ToFlatIndex(cell_index));
  return semantics_counter().at(ToFlatIndex(cell_index)).size();
}

proto::Grid2D ProbabilityGrid::ToProto() const {
  proto::Grid2D result;
  result = Grid2D::ToProto();
  result.mutable_probability_grid_2d();
  return result;
}

// 根据bounding_box对栅格地图进行裁剪到正好包含点云
std::unique_ptr<Grid2D> ProbabilityGrid::ComputeCroppedGrid() const {
  Eigen::Array2i offset;
  CellLimits cell_limits;
  // 根据bounding_box对栅格地图进行裁剪
  ComputeCroppedLimits(&offset, &cell_limits);
  const double resolution = limits().resolution();
  // 重新计算最大值坐标
  const Eigen::Vector2d max =
      limits().max() - resolution * Eigen::Vector2d(offset.y(), offset.x());
  // 重新定义概率栅格地图的大小
  std::unique_ptr<ProbabilityGrid> cropped_grid =
      absl::make_unique<ProbabilityGrid>(
          MapLimits(resolution, max, cell_limits), conversion_tables_);
  // 给新栅格地图赋值
  for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits)) {
    if (!IsKnown(xy_index + offset)) continue;
    cropped_grid->SetProbability(xy_index, GetProbability(xy_index + offset));
  }

  // 返回新地图的指针
  return std::unique_ptr<Grid2D>(cropped_grid.release());
}

// 获取压缩后的地图栅格数据
bool ProbabilityGrid::DrawToSubmapTexture(
    proto::SubmapQuery::Response::SubmapTexture* const texture,
    transform::Rigid3d local_pose) const {
  Eigen::Array2i offset;
  CellLimits cell_limits;
  // 根据bounding_box对栅格地图进行裁剪
  ComputeCroppedLimits(&offset, &cell_limits);
  
  std::string cells;
  // 遍历地图, 将栅格数据存入cells
  for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits)) {
    if (!IsKnown(xy_index + offset)) {
      cells.push_back(0 /* unknown log odds value */);
      cells.push_back(0 /* alpha */);
      cells.push_back(0);
      continue;
    }
    // We would like to add 'delta' but this is not possible using a value and
    // alpha. We use premultiplied alpha, so when 'delta' is positive we can
    // add it by setting 'alpha' to zero. If it is negative, we set 'value' to
    // zero, and use 'alpha' to subtract. This is only correct when the pixel
    // is currently white, so walls will look too gray. This should be hard to
    // detect visually for the user, though.
    // 我们想添加 'delta'，但使用值和 alpha 是不可能的
    // 我们使用预乘 alpha，因此当 'delta' 为正时，我们可以通过将 'alpha' 设置为零来添加它。 
    // 如果它是负数，我们将 'value' 设置为零，并使用 'alpha' 进行减法。 这仅在像素当前为白色时才正确，因此墙壁看起来太灰。 
    // 但是，这对于用户来说应该很难在视觉上检测到。
    
    // delta处于[-127, 127]
    const int8 delta =
        128 - ProbabilityToLogOddsInteger(GetProbability(xy_index + offset));
    //。。。写一个函数 输入索引 输出颜色值const color=GetColor(xy_index + offset)
    const uint8 color = uint8(GetColor(xy_index + offset));
    // 存数据时存了2个值, 一个是栅格值value, 另一个是alpha透明度
    //。。。将颜色值存入cell
    cells.push_back(delta);
    cells.push_back(color);
    cells.push_back(GetSemantics_observer(xy_index + offset));
  }

  // 保存地图栅格数据时进行压缩
  common::FastGzipString(cells, texture->mutable_cells());
  
  // 填充地图描述信息
  texture->set_width(cell_limits.num_x_cells);
  texture->set_height(cell_limits.num_y_cells);
  const double resolution = limits().resolution();
  texture->set_resolution(resolution);
  const double max_x = limits().max().x() - resolution * offset.y();
  const double max_y = limits().max().y() - resolution * offset.x();
  *texture->mutable_slice_pose() = transform::ToProto(
      local_pose.inverse() *
      transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.)));
  
  return true;
}

}  // namespace mapping
}  // namespace cartographer