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;


                    if (center_count <= max_size &&
                        center_count >= min_size &&
                        avg_intensity > 850 &&
                        avg_range > 1 &&
                        avg_range < 6)
                    {

                        //calculate the angle
                        for (int i = 0; i < reflector_data.size(); i++)
                        {
                            center_yaw += (reflector_data[i].first);
                        }
                        center_yaw /= center_count; //average

                        //calculate the distance
                        for (int i = 0; i < reflector_data.size(); i++)
                        {
                            //part_1
                            double theta = fabs(reflector_data[i].first - center_yaw);
                            double ds_1 = reflector_data[i].second * cos(theta);

                            //part_2
                            double a = reflector_data[i].second * sin(theta);
                            double b = reflector_radius;
                            double angle = (a / b) > 1 ? 1.0 : a / b; // Bug fixed!!!
                            double theta_2 = asin(angle);
                            double ds_2 = reflector_radius * cos(theta_2);

                            center_distance += (ds_1 + ds_2);
                        }
                        center_distance /= center_count; //average
                        center_yaw -= M_PI;              //transfer to -180~180

                        Reflector_pos item;
                        item.center_x = center_distance * cos(center_yaw);
                        item.center_y = center_distance * sin(center_yaw);
                        item.center_yaw = center_yaw;
                        reflectors_.push_back(item);
                    }

                    center_x = 0.0;
                    center_y = 0.0;
                    center_count = 0;
                    center_yaw = 0;
                    center_distance = 0;
                    i = j - 1;
                    break;
                }
            }
            last_index = p->index;
        }
    }

    //peak add
    //检测到的反光柱的数量

    cartographer_ros_msgs::LandmarkList reflector_LandMarkList;
    double origin_x, origin_y, origin_theta;
        origin_x = 0;
        origin_y = 0;
        origin_theta = 0;
        Reflector_pos item_j,item_k;
    for (int i = 0; i < reflectors_.size(); i++)
            {
                Reflector_pos item_i = reflectors_[i];

                //log(Info, "i = " + std::to_string(i));
                switch (i)
                {
                case 0 :
                    //log(Info, "-------------------------vetex is 0 --------------------------");
                    item_j = reflectors_[1];
                    item_k = reflectors_[2];
                    break;
                case 1 :
                    //log(Info, "-------------------------vetex is 1 --------------------------");
                    item_j = reflectors_[0];
                    item_k = reflectors_[2];
                    break;
                case 2 :
                    //log(Info, "-------------------------vetex is 2 --------------------------");
                    item_j = reflectors_[0];
                    item_k = reflectors_[1];
                    break;
                }

                double theta_1 = atan2(item_i.center_y - item_j.center_y, item_i.center_x - item_j.center_x);
                //log(Info, "theta_1 is " + std::to_string(theta_1));
                double theta_2 = atan2(item_i.center_y - item_k.center_y, item_i.center_x - item_k.center_x);
                //log(Info, "theta_2 is " + std::to_string(theta_2));

                //degree detect error is not less 20
                double f_theta = fabs(theta_1 - theta_2);
                //log(Info, "f_theta is " + std::to_string(f_theta));
                //log(Info, "fabs(f_theta - M_PI/2) = " + std::to_string(fabs(f_theta - M_PI/2)) );
                if (fabs(f_theta - M_PI/2) > M_PI/9) continue;

                //log(Info, "i = " + std::to_string(i) + " satisfy condition");
                //if satisfy condition (less 20) then
                double x_ij = fabs(item_i.center_x - item_j.center_x);
                double y_ij = fabs(item_i.center_y - item_j.center_y);
                double distance_ij = sqrt(pow(x_ij, 2) + pow(y_ij, 2));
                //log(Info, "distance_ij is = " + std::to_string(distance_ij));
                double x_ik = fabs(item_i.center_x - item_k.center_x);
                double y_ik = fabs(item_i.center_y - item_k.center_y);
                double distance_ik = sqrt(pow(x_ik, 2) + pow(y_ik, 2));
                //log(Info, "distance_ik is = " + std::to_string(distance_ik));

                double long_distance = 0;
                if (distance_ij > distance_ik)
                {
                    //ij is the axis x
                    long_distance = distance_ij;
                    origin_theta = atan2(1, 0) - atan2(item_i.center_y - item_j.center_y, item_i.center_x - item_j.center_x);
                    //log(Info, "origin_theta = " + std::to_string(origin_theta));
                }
                else
                {
                    //ik is the axis_x
                    long_distance = distance_ik;
                    origin_theta = atan2(1, 0) - atan2(item_i.center_y - item_k.center_y, item_i.center_x - item_k.center_x);
                    //log(Info, "origin_theta = " + std::to_string(origin_theta));
                }

                //the long side of right angle is even number;
                if(round_int(long_distance) % 2 != 0) return;
                //log(Info, "long_distance = " + std::to_string(long_distance));
                origin_x = item_i.center_x;
                origin_y = item_i.center_y;
                //log(Info, "origin_x = " + std::to_string(origin_x));
                //log(Info, "origin_y = " + std::to_string(origin_y));

        if(reflector_combination_mode != 1)
        {
            double distance = sqrt(pow(origin_x, 2) + pow(origin_y, 2));
            if (distance < 1.0) return;
            //
            cartographer_ros_msgs::LandmarkEntry reflector_LandMarkEntry;
            reflector_LandMarkEntry.id = "landmark_1";
            reflector_LandMarkEntry.tracking_from_landmark_transform.position.x = reflectors_[i].center_x;
            reflector_LandMarkEntry.tracking_from_landmark_transform.position.y = reflectors_[i].center_y;
            reflector_LandMarkEntry.tracking_from_landmark_transform.position.z = 0;
            tf::Quaternion quat = tf::createQuaternionFromYaw(-1.0 * origin_theta);
            reflector_LandMarkEntry.tracking_from_landmark_transform.orientation.x = quat.x();
            reflector_LandMarkEntry.tracking_from_landmark_transform.orientation.y = quat.y();
            reflector_LandMarkEntry.tracking_from_landmark_transform.orientation.z = quat.z();
            reflector_LandMarkEntry.tracking_from_landmark_transform.orientation.w = quat.w();
            reflector_LandMarkEntry.translation_weight = 20;
            reflector_LandMarkEntry.rotation_weight = 20;
            //reflector_LandMarkEntry.type = "reflector_combined";
            reflector_LandMarkList.header.frame_id = "car_laser";
            reflector_LandMarkList.header.stamp = scan->header.stamp; //ros::Time::now();
            reflector_LandMarkList.landmarks.push_back(reflector_LandMarkEntry);
        }
            }
}


int main(int argc,char *argv[])
{
    //使用反光柱的模式""
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"landmark");
    ros::NodeHandle nh;
    ros::Subscriber sub=nh.subscribe<sensor_msgs::LaserScan>("scan",10,laserProcess);
    ros::spin();
    return 0;
}

2. cartographer中使用

  1. 在CARTOGRAPHER运行配置文件中打开
use_landmarks= true
  1. 运行CARTOGRAPHER。如果其他一切正常,会出现等待LANDMARK的警告,且全局状态会报错。
    在这里插入图片描述

此时命令行窗口显示
在这里插入图片描述
此时程序会挂起,一直等到接收到landmark话题才开始运行。

3. 其他的landmark定位方法

venus的方式是一种纯反光柱的方案,

网址:https://gitee.com/dustinksi/VEnus
视频地址:https://www.bilibili.com/video/av80266705/

4. 参考链接

https://charon-cheung.github.io

https://blog.csdn.net/weixin_43823054/article/details/88881746

https://its401.com/article/yeluohanchan/109909754

https://blog.csdn.net/windxf/article/details/114586328