栅格值从CorrespondenceCostValue到[-1, 100]

本文有部分转自“(02)Cartographer源码无死角解析-(44) 2D栅格地图→ProbabilityGrid 与 ProbabilityToLogOddsInteger()”。

在CreateOccupancyGridMsg，存在个我无法理解的地方。当不是未知栅格时，是用以下公式计算栅格值。

value = cartographer::common::RoundToInt((1. - color / 255.) * 100.);

color越大，value越小。如果color对应intensity，意味着color越大，栅格占用的概率越大；反映到nav_msgs::OccupancyGrid，栅格占用的概率value越小。

从后面导航使用可知，nav_msgs::OccupancyGrid中，当栅格值不是-1时，是栅格值越大，占用的概率越大，那这结果岂不是和上面这个结论相反？

CorrespondenceCostValue到OccupancyGrid的[-1, 100]，可认为经过三个阶段。

1. [ProbabilityGrid::DrawToSubmapTexture]CorrespondenceCostValue分解出两分量：value、alpha。
2. [DrawTexture]从(value、alpha)到(alpha、intensity、observed)。intensity就是value，就是换了变量名。DrawTexture并没有数值上有过改变，只是多出一个分量observed，专门表示是否有观测到过hit或miss，如果等于0，即没观测过，那就是未知。
3. [CreateOccupancyGridMsg]从(alpha、intensity、observed)到一字节的值。生成时只须要(intensity、observed)。

概率栅格图(ProbabilityGrid)中“点云画到概率图”写过个结论：只要在ProbabilityGridRangeDataInserter2D::Insert后，栅格值范围就限制在了[0, kUpdateMarker-1]。于是可知道，在同线程、LocalTrajectoryBuilder2D::AddRangeData函数外的，可认为概率图中栅格值范围限制在[0, kUpdateMarker-1]。而在后面cartographer_occupancy_grid_node结点发布的栅格图可知道，那里的栅格值范围是[-1, 100]，那这两范围是怎么转换的。

一、cartographer_node节点的Node::HandleSubmapQuery

要没意外，使用cartographer须至少运行两个节点：cartographer_node、cartographer_occupancy_grid。

cartographer_occupancy_grid节点会订阅“submap_list”话题。在处理收到一个“submap_list”话题时，对应调用Node::HandleSubmapList(const cartographer_ros_msgs::SubmapList::ConstPtr& msg)，主要任务是根据子图列表生成submap_slices_。对得到参数msg，写着当前所有子图。考虑到效率，这些子图只写着关键信息，像{trajectory_id, submap_index}，不包括需较多内存的栅格数据。cartographer_occupancy_grid接下要得到该子图栅格数据时，需要以{trajectory_id, submap_index}为参数，发出“submap_query”action请求。

另一方面，是cartographer_node节点在处理“submap_query”action请求，处理函数是图1中“Call Stack”高亮的Node::HandleSubmapQuery。图1可看到Node::HandleSubmapQuery是运行在cartographer_node节点主线程，这也是运行LocalTrajectoryBuilder2D::AddRangeData的线程。既然同一线程，就不必担心概率图中栅格数据读、写同步。而且可以确定，Node::HandleSubmapQuery看到的概率图中栅格值范围已限制在[0, kUpdateMarker-1]。

接下就从ProbabilityGrid::DrawToSubmapTexture开始让看这范围变换。

1.1 ProbabilityGrid::DrawToSubmapTexture()

DrawToSubmapTexture会以着std::string存储栅格数据，源码中创建了变量std::string cells。其中每2字节描述一个cell：

1. 第一个字节：栅格值value。0 <= value <=127，越接近127，表示栅格占用的概率越大。
2. 第二个字节：alpha透明度。0 <= alpha <=127，越接近127，表示栅格空闲的概率越大。
3. value和alpha都是0时，表示该cell是未知。

生成的cells放在texture->cells_字段。

// 获取压缩后的地图栅格数据
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;

调用父类 ComputeCroppedLimits(&offset, &cell_limits) 函数，对栅格地图进行剪裁。创建一个 std::string 对象 cells。然后对剪切之后地图所有cell进行遍历。

  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 */);

此个栅格状态是kUnknownCorrespondenceValue，也就是说，一直到现既没有观测到过hit，也没有miss。把该 cell 的两个字节都设置为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 int delta =
        128 - ProbabilityToLogOddsInteger(GetProbability(xy_index + offset));

此个栅格至少观测到过一次hit或miss，那么首先获取该cell被占用的概率值，通过 ProbabilityToLogOddsInteger() 函数把其映射到 [1, 255] 之间，然后再映射到[-127, 127]，使用变量 delta 表示。关于 ProbabilityToLogOddsInteger() 函数后面单独分析。

    const uint8 alpha = delta > 0 ? 0 : -delta;
    const uint8 value = delta > 0 ? delta : 0;
    // 存数据时存了2个值, 一个是栅格值value, 另一个是alpha透明度
    cells.push_back(value);
    cells.push_back((value || alpha) ? alpha : 1);

delta的范围在[-127, 127]之间。

• delta > 0时，越接近127，表示 cell占用的概率越大。alpha = 0，value = delta
• delta < 0时，越接近-127，表示 cell空闲的概率越大。alpha = -delta，value = 0。
• delta == 0时，alpha=1，value=0。两个const时的赋值是alpha=0，但在push_back时会把alpha改为1。

只要额过观测值，不论alpha还是value，它们总是>=0，即不会出现负数。而且总是一个0，一个非0，又不可能同是0。正如上面“!IsKnown”滤掉的，如果alpha、value都是0，那是表示该栅格未知。

value越接近127，表示 cell占用的概率越大。

alpha越接近127，表示 cell空闲的概率越大。

  }   

  // 保存地图栅格数据时进行压缩
  common::FastGzipString(cells, texture->mutable_cells());

把所有cell的栅格信息以 value 与 alpha 的形式存储到 std::string cells 之后，会调用 common::FastGzipString 对栅格地图数据进行压缩，压缩结果存储于 texture 之中。

  // 填充地图描述信息
  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.)));

填充地图信息，如宽高cell数，分辨率等等最后调用了 *texture->mutable_slice_pose() 函数进行赋值，这里没有看明白，先记一下。

  return true;
}

2.1 ProbabilityToLogOddsInteger()

现在呢，我们回过头来看一下 ProbabilityGrid::DrawToSubmapTexture() 中调用的 ProbabilityToLogOddsInteger() 函数，该函数实现于 src/cartographer/cartographer/mapping/submaps.h 文件中，其主要原理可以阅读 Cartographer 论文：“Real-Time Loop Closure in 2D LIDAR SLAM”。

// Converts the given probability to log odds.
// 对论文里的 odds(p)函数 又取了 log
inline float Logit(float probability) {
  return std::log(probability / (1.f - probability));
}

const float kMaxLogOdds = Logit(kMaxProbability);
const float kMinLogOdds = Logit(kMinProbability);

// Converts a probability to a log odds integer. 0 means unknown, [kMinLogOdds,
// kMaxLogOdds] is mapped to [1, 255].
inline uint8 ProbabilityToLogOddsInteger(const float probability) {
  const int value = common::RoundToInt((Logit(probability) - kMinLogOdds) *
                                       254.f / (kMaxLogOdds - kMinLogOdds)) +
                    1;
  CHECK_LE(1, value);
  CHECK_GE(255, value);
  return value;
}

首先来看 Logit 这个函数，对std::log内中参数，，这个公式和probability_values.h中的“float Odds(float probability)”一模一样。的确，两处有着一样的物理意义。

这个odds表示occupied概率与free概率的比值，这里抄下“probability_values.h/c：占据概率相关”对odds的解释。

• Odd(s)值等于1时，表征一半对一半，该点被occupied和free的概率各为0.5。
• 如果Odd(s)值大于1，表征该点被occupied的概率更大；Odd(s)越大，occupied的概率越大。范围为1~
• 如果Odd(s)值小于1，表征该点free的概率更大；Odd(s)越小，free的概率越大。范围为0~1。

odds范围是0~，把std::log(odds)值记为，log函数(e为底) 的性质，范围是~。

很显然，直接使用是不合适的，因为其区间为。论文中使用了一个巧妙的办法，那就 probability 最大值为0.9，那么未被占用的概率最小为0.1，现在回过头来看 probability_values.h 文件中定义的如下变量：

constexpr float kMinProbability = 0.1f;                         // 0.1
constexpr float kMaxProbability = 1.f - kMinProbability;        // 0.9
constexpr float kMinCorrespondenceCost = 1.f - kMaxProbability; // 0.1
constexpr float kMaxCorrespondenceCost = 1.f - kMinProbability; // 0.9

就比较好理解了，ProbabilityToLogOddsInteger()函数由于输入 probability 只能在 [01, 0.9] 之间取值，所以可以把夹紧到区间 [kMinLogOdds, kMaxLogOdds]。最后再把区间映射到 [1, 255] 区间上。这样就达到了 ProbabilityToLogOddsInteger() 函数的最终目的。

二、cartographer_occupancy_grid节点的Node::HandleSubmapList

上面有说到，cartographer_occupancy_grid节点会订阅“submap_list”话题。在处理收到一个“submap_list”话题时，对应调用Node::HandleSubmapList(const cartographer_ros_msgs::SubmapList::ConstPtr& msg)。图2中“Call Stack”高亮的Node::HandleSubmapList显示了调用情况。

2.1 Node::HandleSubmapList

HandleSubmapList负责根据子图列表(msg)生成submap_slices_，处理后，submap_slices_已将概率图转成cario图面，但没有将子图拼成大地图。处理过程中，“submap_list”给出了子图索引、绝对位姿、概率图版本，但没有该子图概率图（栅格数据）。一旦没有得到过该子图的概率图或概率图版本发生变化，会以子图索引为参数调用“submap_query”服务获得概率图。

<cartographer_ros>/cartographer_ros/occupancy_grid_node_main.cc
------
void Node::HandleSubmapList(
    const cartographer_ros_msgs::SubmapList::ConstPtr& msg) {
  absl::MutexLock locker(&mutex_);

  // We do not do any work if nobody listens.
  if (occupancy_grid_publisher_.getNumSubscribers() == 0) {
    return;
  }

  // Keep track of submap IDs that don't appear in the message anymore.
  std::set<SubmapId> submap_ids_to_delete;
  for (const auto& pair : submap_slices_) {
    submap_ids_to_delete.insert(pair.first);
  }

经过HandleSubmapList后，submap_slices_数的子图情况必须msg->submap的保持一致。一旦这子图是在msg->submap中的，那它会从submap_ids_to_delete中栅除。一旦循环完整个msg->submap，依旧留在submap_ids_to_delete中的，那就是之前有、但现在没有了的子图，需要删除。

  for (const auto& submap_msg : msg->submap) {
    const SubmapId id{submap_msg.trajectory_id, submap_msg.submap_index};
    submap_ids_to_delete.erase(id);
    if ((submap_msg.is_frozen && !FLAGS_include_frozen_submaps) ||
        (!submap_msg.is_frozen && !FLAGS_include_unfrozen_submaps)) {
      continue;
    }
    SubmapSlice& submap_slice = submap_slices_[id];
    submap_slice.pose = ToRigid3d(submap_msg.pose);
    submap_slice.metadata_version = submap_msg.submap_version;
    if (submap_slice.surface != nullptr &&
        submap_slice.version == submap_msg.submap_version) {
      continue;
    }

    auto fetched_textures =
        ::cartographer_ros::FetchSubmapTextures(id, &client_);

这个标识id的子图，它或没在之前的submap_slices_，或发生版本变换了，需以子图索引为参数调用“submap_query”服务获得该详细数据。

FetchSubmapTextures得到“submap_query”应答后，会将std::string类型cells解压缩，对应恢复出values、alphas数组，不过它把value变量名改成了intensity。

    if (fetched_textures == nullptr) {
      continue;
    }
    CHECK(!fetched_textures->textures.empty());
    submap_slice.version = fetched_textures->version;

    // We use the first texture only. By convention this is the highest
    // resolution texture and that is the one we want to use to construct the
    // map for ROS.

    const auto fetched_texture = fetched_textures->textures.begin();
    submap_slice.width = fetched_texture->width;
    submap_slice.height = fetched_texture->height;
    submap_slice.slice_pose = fetched_texture->slice_pose;
    submap_slice.resolution = fetched_texture->resolution;
    submap_slice.cairo_data.clear();
    submap_slice.surface = ::cartographer::io::DrawTexture(
        fetched_texture->pixels.intensity, fetched_texture->pixels.alpha,
        fetched_texture->width, fetched_texture->height,
        &submap_slice.cairo_data);

fetched_texture->pixels.intensity、fetched_texture->pixels.alpha分别对应着DrawToSubmapTexture中的value、alpha数组。见图2，数组长度都是“fetched_texture->width*fetched_texture->height”。经过DrawTexture，栅格值表示成三个部分：intensity、alpha、observed，然后每个部以1字节封装成一个uint32_t。

  }

  // Delete all submaps that didn't appear in the message.
  for (const auto& id : submap_ids_to_delete) {
    submap_slices_.erase(id);

循环完整个msg->submap后，依旧留在submap_ids_to_delete中的，那就是之前有、但现在没有了的子图，需要删除。

  }

  last_timestamp_ = msg->header.stamp;
  last_frame_id_ = msg->header.frame_id;
}

2.2 DrawTexture

DrawTexture负责把以(intensity, alplha)对表示的栅格值转换成(intensity, alplha, observed)，并以着每分量1字节封装一个uint32_t。

• [IN]intensity。value数组。长度width*height。
• [IN]alpha。alpha数组。长度width*height。
• [IN]width、height。该子图以栅格为单元尺寸。
• [OUT]cairo_data。存储生成的栅格值。

<cartographer>/cartographer/io/submap_painter.cc
------
UniqueCairoSurfacePtr DrawTexture(const std::vector<char>& intensity,
                                  const std::vector<char>& alpha,
                                  const int width, const int height,
                                  std::vector<uint32_t>* const cairo_data) {
  CHECK(cairo_data->empty());

  // Properly dealing with a non-common stride would make this code much more
  // complicated. Let's check that it is not needed.
  const int expected_stride = 4 * width;
  CHECK_EQ(expected_stride, cairo_format_stride_for_width(kCairoFormat, width));
  for (size_t i = 0; i < intensity.size(); ++i) {
    // We use the red channel to track intensity information. The green
    // channel we use to track if a cell was ever observed.
    const uint8_t intensity_value = intensity.at(i);
    const uint8_t alpha_value = alpha.at(i);
    const uint8_t observed =
        (intensity_value == 0 && alpha_value == 0) ? 0 : 255;
    cairo_data->push_back((alpha_value << 24) | (intensity_value << 16) |
                          (observed << 8) | 0);

• b31..b24：alpha，DrawToSubmapTexture中的value。0 <= alpha <=127，越接近127，表示栅格空闲的概率越大。
• b23..b16：intensity。0 <= value <=127，越接近127，表示栅格占用的概率越大。
• b15..b8：observed。只有两种值：0、255。指示是否在该观测到过hit或miss。是255时，表示有观测到过，此时alpha、intensity至少有一个非0。是0时，表示没未观测到过，此时alpha、intensity都是0。
• b7..b0：固定0。

  }

  auto surface = MakeUniqueCairoSurfacePtr(cairo_image_surface_create_for_data(
      reinterpret_cast<unsigned char*>(cairo_data->data()), kCairoFormat, width,
      height, expected_stride));
  CHECK_EQ(cairo_surface_status(surface.get()), CAIRO_STATUS_SUCCESS)
      << cairo_status_to_string(cairo_surface_status(surface.get()));
  return surface;
}

相比DrawToSubmapTexture，DrawTexture并没有数值上有过改变，只是多出一个分量observed，专门表示是否有观测到过hit或miss，如果等于0，即没观测过，那就是未知。

三、cartographer_occupancy_grid节点的Node::DrawAndPublish

cartographer_occupancy_grid节点会创建间隔publish_period_sec秒(不是lua配置，默认1秒)的定时器，处理例程Node::DrawAndPublish，用于定时发布“map”话题。

图3显示了DrawAndPublish函数栈情况。以下是它依次执行的工作。

1. [PaintSubmapSlices]把submap_slices_中子图拼成大地图，结果存放在painted_slices。
2. [CreateOccupancyGridMsg]把painted_slices转成msgs::OccupancyGrid，结果放在msg_ptr。
3. [.publish(*msg_ptr)]基于msg_ptr发布个“map”话题。

“发布地图（map话题）”，说了第一步骤PaintSubmapSlices。针对栅格值变换，在把子图拼成大地图时，应该也会涉及到。毕竟，同一个栅格，可能会存在多个子图，那最后栅格值是什么个情况？

我不熟悉cairo库，这须要有专业人给出答案。这里就直接猜了：不会改变(intensity, alplha, observed)意义，但生成的栅格值是多子图中各值的和。——但这结论极可能是错的，因为按这理解的话，会无法理解接下会出现的用“(1. - color / 255.) * 100.”计算nav_msgs::OccupancyGrid栅格值。

3.1 CreateOccupancyGridMsg

<cartographer_ros>/cartographer_ros/cartographer_ros/msg_conversion.cc
------
std::unique_ptr<nav_msgs::OccupancyGrid> CreateOccupancyGridMsg(
    const cartographer::io::PaintSubmapSlicesResult& painted_slices,
    const double resolution, const std::string& frame_id,
    const ros::Time& time) {
  auto occupancy_grid = absl::make_unique<nav_msgs::OccupancyGrid>();

  const int width = cairo_image_surface_get_width(painted_slices.surface.get());
  const int height =
      cairo_image_surface_get_height(painted_slices.surface.get());
  ...
  const uint32_t* pixel_data = reinterpret_cast<uint32_t*>(
      cairo_image_surface_get_data(painted_slices.surface.get()));
  occupancy_grid->data.reserve(width * height);
  for (int y = height - 1; y >= 0; --y) {
    for (int x = 0; x < width; ++x) {
      const uint32_t packed = pixel_data[y * width + x];
      const unsigned char color = packed >> 16;
      const unsigned char observed = packed >> 8;
      const int value =
          observed == 0
              ? -1
              : ::cartographer::common::RoundToInt((1. - color / 255.) * 100.);

kCairoFormat是CAIRO_FORMAT_ARGB32。一个像素占4个字节。

observed只有两种值：0、255。指示是否在该栅格观测到过hit或miss。

• 是0时，表示没观测到过。于是它等于0时，value=-1，表示该栅格未知。
• 是255时，表示有观测到过。用表示占用概率的color(intensity)来计算要放入map的栅格值。对单个子图来说，0 <= color <=127，但如果该栅格存在多个子图，那此时的color可能是超过127。但不管怎么说，此此算出的value范围是[0, 100]。

结合observed两种情况，最终算出的value范围是[-1, 100]。-1表示未知，即从没有在该栅格观测到过hit和miss。但对不是未知栅格时，用的计算栅格值公式，我无法理解，这和认为就是相反的。

      CHECK_LE(-1, value);
      CHECK_GE(100, value);
      occupancy_grid->data.push_back(value);
    }
  }

  return occupancy_grid;
}

贴完CreateOccupancyGridMsg，说下我无法理解的一个地方。当不是未知栅格时，是用以下公式计算栅格值。

value = cartographer::common::RoundToInt((1. - color / 255.) * 100.);

color越大，value越小。如果color对应intensity，意味着color越大，栅格占用的概率越大；反映到nav_msgs::OccupancyGrid，栅格占用的概率越小。

从后面导航使用可知，nav_msgs::OccupancyGrid中，当栅格值不是-1时，是栅格值越大，占用的概率越大，那这结果岂不是和上面这个结论相反？

nav_msgs::OccupancyGrid中栅格值范围是[-1, 100]。-1到costmap2d后是costmap_2d::NO_INFORMATION，100往往是costmap_2d::LETHAL_OBSTACLE。但栅格在一个子图中最多127，

实际使用时，栅格值往往达不到100。这样一来，如果用变量lethal_threshold依止使用默认值100的话，导航包中静态层栅格很少会在膨胀层产生膨胀。“修改cartographer源代码使其发布ros标准格式的占据栅格地图”写的是把这个上限由100改到50。