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中使用
- 在CARTOGRAPHER运行配置文件中打开
use_landmarks= true
- 运行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
评论(28)
您还未登录,请登录后发表或查看评论