0. 简介

反光板定位作为cartographer中landmark数据最常用的部分，其特性和AprilTag使用方法类似，在cartographer中，landmark必须是tracking frame，一般是imu、laser、camera或者base_footprint。如果提供landmark observations不低于10 Hz，那么可以设置TRAJECTORY_BUILDER.collate_landmarks = on. Cartographer将deterministically运行(对于给定的bag, 使用offline node每次获得的结果是一样的). 如果collate_landmarks = off， landmark observations将跳过sensor queue (so they do not block it if they are sparse) and are injected directly into the pose graph, which is not deterministic.

1. cartographer中的landmark

cartographer在跑纯定位的时候，判断定位丢失的标准是一个比较棘手的事情。在cartographer中经常使用的是scan_to_map的方式，通过cartographer自己维护的PoseExtrapolator使用匹配的方式来获得当前帧与地图的匹配值，但是这样的方式在环境经常变的场景下效果会比较差， 而使用landmark可以有效地避免这类问题的发生，还是会出现坐标的跳变，从一个位置跳到了另外一个位置，这种情况也会发生在环境变化不大的场景中。这时候使用landmark进行标注则是非常重要的。这种landmark方法基本是作为定位的修正补充，通过建立一个最小二乘的数学模型，来迭代计算，给优化增加一些元素。在室内场景下来说，这种方式还是比较鲁棒的，而且在此基础上可以做一个融合。

这里基于一些开源的程序以及自己的理解分享一波反光板landmark的功能，代码如下:

#include "ros/ros.h"
#include "cartographer_ros_msgs/LandmarkList.h"
#include "cartographer_ros_msgs/LandmarkEntry.h"
#include "geometry_msgs/Pose.h"
#include "std_msgs/String.h"
#include "std_msgs/Header.h"
#include "sensor_msgs/LaserScan.h"

//添加的部分
#include <tf/tf.h>

//添加的部分
int last_index = 0;    
double reflector_radius = 0.0 / 2.0;
int min_reflector_sample_count = 8;
int reflector_combination_mode = 0;

struct Sample
{
  int   index;
  float range;
  float intensity;
  float x;
  float y;
};

//添加的部分
struct Reflector_pos {

    double center_x;
    double center_y;
    double center_yaw;

    bool operator <(const Reflector_pos& other) const {
        double this_distance = hypot(center_x,center_y);
        double other_distance = hypot(other.center_x,other.center_y);

        return this_distance < other_distance;
    }
};


Sample* Extract(int ind, int inensity, const sensor_msgs::LaserScan& scan)
{
  Sample* s=new Sample();

  s->index = ind;
  s->range = scan.ranges[ind];
  s->intensity = scan.intensities[ind];
  s->x = cos(scan.angle_min + ind * scan.angle_increment) * s->range;
  s->y = sin(scan.angle_min + ind * scan.angle_increment) * s->range;
  if (     s->range > scan.range_min
        && s->range < scan.range_max
        && s->intensity > inensity
           )
    return s;
  else
  {
    delete s;
    return NULL;
  }
}

//添加的部分
int round_int(double val)
    {
        return (val > 0.0) ? floor(val + 0.5) : ceil(val - 0.5);
    }


void laserProcess(const sensor_msgs::LaserScanConstPtr &scan)
{
    int min_reflector_intensity=0.5;
    tf::Pose laser_pose;
    std::vector<Sample *> scan_filter;
    ros::param::get("/laser_reflector_detect_node/reflector_combination_mode", reflector_combination_mode);//..................................................

    for (uint32_t i = 0; i < scan->ranges.size(); i++)
    {
        Sample *s = Extract(i, min_reflector_intensity, *scan);
        if (s != NULL) scan_filter.push_back(s);
    }

    if (scan_filter.size() == 0) return;

    //get the center of reflector
    double center_x, center_y, center_yaw, center_count, center_distance;
    std::vector<Reflector_pos> reflectors_;
    bool is_mark_start = true;

    // store the temp data of reflector
    std::vector<std::pair<double, double>> reflector_data; //first is angle, second is distance;
    // store the intensity data of reflector
    std::vector<int> intensity_data;

    //std::cout << "peak.ding scan_filter size " << scan_filter.size() << std::endl;
    //检测有多少个反光柱
    for (int i = 0; i < scan_filter.size(); i++)
    {
        is_mark_start = true;

        for (int j = i; j < scan_filter.size(); j++)
        {
            Sample *p = scan_filter[j];

            double item_r = p->range;
            double item_i = p->intensity;
            //origin angle range is -180~180,transfer to 0~360
            double item_a = scan->angle_min + p->index * scan->angle_increment + M_PI;

            if (is_mark_start == true)
            {
                //center_x = p->x;
                //center_y = p->y;
                //center_yaw = item_angle;
                center_count = 1;
                //开始记录单个反光柱的角度和距离
                reflector_data.clear();
                reflector_data.push_back(std::pair<double, double>(item_a, item_r));
                //std::cout << "Peak.ding index " << p->index << " item_angle " << item_a << std::endl;
                //开始记录单个反光柱的强度
                intensity_data.clear();
                intensity_data.push_back(item_i);

                is_mark_start = false;
            }
            else
            {
                //检测强度点是否连续来进行分割
                if ((p->index - last_index) < 3 && (j != scan_filter.size() - 1))
                {
                    reflector_data.push_back(std::pair<double, double>(item_a, item_r));

                    intensity_data.push_back(item_i);
                    center_count++;
                }
                else
                {
                    //记录当前的反光柱点平均距离
                    double sum_range_of_target = 0;
                    for (auto it : reflector_data)
                    {
                        sum_range_of_target += it.second;
                    }
                    double avg_range = sum_range_of_target / reflector_data.size();

                    //记录当前反光柱子的平均强度
                    double sum_intensity_of_target = 0;
                    for (auto it : intensity_data)
                    {
                        sum_intensity_of_target += it;
                    }
                    double avg_intensity = sum_intensity_of_target / intensity_data.size();

                    //check the points number of reflecor size is < 5cm/D/scan->angle_increment
                    //计算反光柱的有效点数量 放宽至理想值的0.6~1.2。
                    int max_size = std::floor((2.0 * reflector_radius / avg_range) / scan->angle_increment * 1.2);
                    int min_size = max_size * 0.6 > min_reflector_sample_count ? max_size * 0.6 : min_reflector_sample_count;

…详情请参照古月居