激光扫描的更新生成概率地图

Previous

1.首先获取到的是laser scan 的信息，然后想得到一个更新概率地图的一个pineline

输入就是一个laserscan 输出就是个栅格概率地图

2.参考cartographer/ occupancy grid node

Learning

cartographer_ros/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc

#include "cartographer/io/image.h"

/*
 * Copyright 2017 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.
 */

#ifndef CARTOGRAPHER_IO_IMAGE_H_
#define CARTOGRAPHER_IO_IMAGE_H_

#include <cstdint>
#include <vector>

#include "cairo/cairo.h"
#include "cartographer/common/port.h"
#include "cartographer/io/color.h"
#include "cartographer/io/file_writer.h"
#include "cartographer/io/points_batch.h"

namespace cartographer {
namespace io {

// The only cairo image format we use for Cartographer.
constexpr cairo_format_t kCairoFormat = CAIRO_FORMAT_ARGB32;

// std::unique_ptr for Cairo surfaces. The surface is destroyed when the
// std::unique_ptr is reset or destroyed.
using UniqueCairoSurfacePtr =
    std::unique_ptr<cairo_surface_t, void (*)(cairo_surface_t*)>;

// Takes ownership.
UniqueCairoSurfacePtr MakeUniqueCairoSurfacePtr(cairo_surface_t* surface);

// std::unique_ptr for Cairo contexts.
using UniqueCairoPtr = std::unique_ptr<cairo_t, void (*)(cairo_t*)>;

// Takes ownership.
UniqueCairoPtr MakeUniqueCairoPtr(cairo_t* surface);

class Image {
 public:
  explicit Image(UniqueCairoSurfacePtr surface);
  Image(int width, int height);

  const Uint8Color GetPixel(int x, int y) const;
  void SetPixel(int x, int y, const Uint8Color& color);
  void WritePng(FileWriter* const file_writer);

  // Rotates the image in place.
  void Rotate90DegreesClockwise();

  // Returns a pointer to a cairo surface that contains the current pixel data.
  // The 'Image' object must therefore outlive the returned surface object. It
  // is undefined behavior to call any of the mutating functions while a pointer
  // to this surface is alive.
  UniqueCairoSurfacePtr GetCairoSurface();

  int width() const { return width_; }
  int height() const { return height_; }

 private:
  int width_;
  int height_;
  std::vector<uint32> pixels_;
};

}  // namespace io
}  // namespace cartographer

#endif  // CARTOGRAPHER_IO_IMAGE_H_

image.h主要是用到了一个叫做cairo的库

Surfaces: Cairo: A Vector Graphics Library - cairographics.org

看名字是一个处理矢量图的库，还得使用这个库跟cartographer的IO链接。

class Node {
 public:
  explicit Node(double resolution, double publish_period_sec);
  ~Node() {}

  Node(const Node&) = delete;
  Node& operator=(const Node&) = delete;

 private:
  void HandleSubmapList(const cartographer_ros_msgs::SubmapList::ConstPtr& msg);
  void DrawAndPublish(const ::ros::WallTimerEvent& timer_event);
  void PublishOccupancyGrid(const std::string& frame_id, const ros::Time& time,
                            const Eigen::Array2f& origin,
                            cairo_surface_t* surface);

  ::ros::NodeHandle node_handle_;
  const double resolution_;

  ::cartographer::common::Mutex mutex_;
  ::ros::ServiceClient client_ GUARDED_BY(mutex_);
  ::ros::Subscriber submap_list_subscriber_ GUARDED_BY(mutex_);
  ::ros::Publisher occupancy_grid_publisher_ GUARDED_BY(mutex_);
  std::map<SubmapId, SubmapSlice> submap_slices_ GUARDED_BY(mutex_);
  ::ros::WallTimer occupancy_grid_publisher_timer_;
  std::string last_frame_id_;
  ros::Time last_timestamp_;
};

先看下这个submap list吧

 PublishOccupancyGrid 本来想找这个函数的function body 结果没有 在GitHub 全局搜索 搜到了许多有趣的。。