cartographer如何使用2个反光柱子作为一个landmark

2个反光柱的摆放要求：靠着墙摆，因为我们只能从一侧看到2个反光柱，从两侧都看到至少需要3个反光柱来定位。
landmark检测原理：因为我们是从一侧看到2个反光柱，所以看到2个反光柱在一帧激光数据中会有顺序，有2种情形，如下图
这两种情形，我们发现
情形1: 这种情形2个反光柱子被检测到的激光角度小于180，我们认为大一些激光角度的是原点。
情形2: 这种情形2个反光柱子被检测到的激光角度大于180，我们认为小一些激光角度的是原点。
这样原点就固定下来，并是唯一的。2个反光柱构成的坐标系也就确定了。和激光坐标系之间的旋转关系可以通过2个向量之间的夹角计算得出。再补充一张图：

talk is cheap, show my code first
代码如下：

    //use two reflector as a landmark//log(Info, "reflectors_ size " + std::to_string(reflectors_.size() ));if (reflectors_.size() < 2)return;for (int i = 0; i < reflectors_.size(); i++){Reflector_pos item_i = reflectors_[i];//log(Info, "item_i.center_yaw : " + std::to_string(item_i.center_yaw));for (int j = i + 1; j < reflectors_.size(); j++){Reflector_pos item_j = reflectors_[j];//log(Info, "item_j.center_yaw : " + std::to_string(item_j.center_yaw));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, "item_i.center_x : " + std::to_string(item_i.center_x));//log(Info, "item_i.center_y : " + std::to_string(item_i.center_y));//log(Info, "item_j.center_x : " + std::to_string(item_j.center_x));//log(Info, "item_j.center_y : " + std::to_string(item_j.center_y));//log(Info, "x_ij : " + std::to_string(x_ij));//log(Info, "y_ij : " + std::to_string(y_ij));//log(Info, "distance_ij : " + std::to_string(distance_ij));//if two reflectors not arrange by special mode, jump outif (fabs(distance_ij - reflector_combined_length) > 0.2)continue;double distance_i = sqrt(pow(item_i.center_x,2) + pow(item_i.center_y,2));double distance_j = sqrt(pow(item_j.center_x,2) + pow(item_j.center_y,2));double coord_x, coord_y, coord_theta;double origin_x, origin_y, origin_theta;if (fabs(item_i.center_yaw - item_j.center_yaw) < M_PI){if (item_i.center_yaw > item_j.center_yaw){//item_i is origindouble up_value = pow(distance_i, 2) + pow(distance_ij, 2) - pow(distance_j, 2);double down_value = 2 * distance_i * distance_ij;double cos_theta = acos(up_value / down_value);coord_x = cos(cos_theta) * distance_i;coord_y = sin(cos_theta) * distance_i;origin_x = item_i.center_x;origin_y = item_i.center_y;origin_theta = atan2(1, 0) - atan2(item_j.center_y - item_i.center_y, item_j.center_x - item_i.center_x);if(origin_theta > M_PI){origin_theta -= 2*M_PI;}if(origin_theta < -M_PI){origin_theta += 2*M_PI;}}else{//item_j is origindouble up_value = pow(distance_j, 2) + pow(distance_ij, 2) - pow(distance_i, 2);double down_value = 2 * distance_j * distance_ij;double cos_theta = acos(up_value / down_value);coord_x = cos(cos_theta) * distance_j;coord_y = sin(cos_theta) * distance_j;coord_theta = atan2(coord_y, coord_x);origin_x = item_j.center_x;origin_y = item_j.center_y;origin_theta = atan2(1, 0) - atan2(item_i.center_y - item_j.center_y, item_i.center_x - item_j.center_x);if(origin_theta > M_PI){origin_theta -= 2*M_PI;}if(origin_theta < -M_PI){origin_theta += 2*M_PI;}}}else{if (item_i.center_yaw < item_j.center_yaw){//item_i is origindouble up_value = pow(distance_i, 2) + pow(distance_ij, 2) - pow(distance_j, 2);double down_value = 2 * distance_i * distance_ij;double cos_theta = acos(up_value / down_value);coord_x = cos(cos_theta) * distance_i;coord_y = sin(cos_theta) * distance_i;coord_theta = atan2(coord_y, coord_x);origin_x = item_i.center_x;origin_y = item_i.center_y;origin_theta = atan2(1, 0) - atan2(item_j.center_y - item_i.center_y, item_j.center_x - item_i.center_x);if(origin_theta > M_PI){origin_theta -= 2*M_PI;}if(origin_theta < -M_PI){origin_theta += 2*M_PI;}}else{//item_j is origindouble up_value = pow(distance_j, 2) + pow(distance_ij, 2) - pow(distance_i, 2);double down_value = 2 * distance_j * distance_ij;double cos_theta = acos(up_value / down_value);coord_x = cos(cos_theta) * distance_j;coord_y = sin(cos_theta) * distance_j;coord_theta = atan2(coord_y, coord_x);origin_x = item_j.center_x;origin_y = item_j.center_y;origin_theta = atan2(1, 0) - atan2(item_i.center_y - item_j.center_y, item_i.center_x - item_j.center_x);if(origin_theta > M_PI){origin_theta -= 2*M_PI;}if(origin_theta < -M_PI){origin_theta += 2*M_PI;}}}//log(Info, "origin_x : " + std::to_string(origin_x));//log(Info, "origin_y : " + std::to_string(origin_y));//log(Info, "origin_theta : " + std::to_string(origin_theta));/*Reflector_pos item;item.center_x = (item_i.center_x + item_j.center_x) / 2;item.center_y = (item_i.center_y + item_j.center_y) / 2;item.center_yaw = atan2(item.center_y , item.center_x);// + M_PI_2;log(Info, "item_i.center_yaw : " + std::to_string(item_i.center_yaw));log(Info, "item_j.center_yaw : " + std::to_string(item_j.center_yaw));//log(Info, "item.center_yaw : " + std::to_string(item.center_yaw));//publish the center pointsgeometry_msgs::PointStamped point;point.header.stamp = scan->header.stamp;point.header.frame_id = "car_laser";point.point.x = item.center_x;point.point.y = item.center_y;point.point.z = 0;reflector_points_.publish(point);*/double distance = sqrt(pow(coord_x, 2) + pow(coord_y, 2));if (distance < 1.0)continue;//cartographer_ros_msgs::LandmarkEntry reflector_LandMarkEntry;reflector_LandMarkEntry.id = "landmark_1";reflector_LandMarkEntry.tracking_from_landmark_transform.position.x = origin_x;reflector_LandMarkEntry.tracking_from_landmark_transform.position.y = origin_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 = 1.0;reflector_LandMarkEntry.rotation_weight = 1.0;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);#ifdef VISUAL_RVIZvisualization_msgs::Marker landmark_item;landmark_item.pose.position.x = origin_x;landmark_item.pose.position.y = origin_y;landmark_item.pose.orientation.x = 0.0;landmark_item.pose.orientation.y = 0.0;landmark_item.pose.orientation.z = 0.0;landmark_item.pose.orientation.w = 1.0;landmark_item.header.frame_id = "car_laser";landmark_item.header.stamp = scan->header.stamp; //ros::Time::now();landmark_item.scale.x = kLandmarkMarkerScale;landmark_item.scale.y = kLandmarkMarkerScale;landmark_item.scale.z = kLandmarkMarkerScale;landmark_item.type = visualization_msgs::Marker::SPHERE;landmark_item.ns = "Landmarks";landmark_item.id = reflector_LandMarkList.landmarks.size();landmark_item.color.a = 1.0;landmark_item.color.r = 0;landmark_item.color.g = 255;landmark_item.color.b = 0;landmark_item.lifetime = ros::Duration(0.05);landmark_poses_list.markers.push_back(landmark_item);
#endif}}

开源代码
https://gitee.com/laser_reflector_detect

cartographer如何使用2个反光柱子作为一个landmark相关推荐

cartographer如何使用3个反光柱作为一个landmark
3个反光柱的摆放规则:摆成一个直角三角形或者直线或其他特征 ,直角边不同即可,可以设置边的长度来作为附加特征. 原理:检测3个顶点的角度,检测到是直角时可以确定该点是landmark坐标系的原点.然后 ...
cartographer中的反光板定位
0. 简介反光板定位作为cartographer中landmark数据最常用的部分,其特性和AprilTag使用方法类似,在cartographer中,landmark必须是tracking fra ...
图像反光能被一键去除了？港科大开源RFC，仅用一个操作，强反光也能完美去除｜CVPR2021...
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者丨雷晨阳审稿丨邓富城编辑丨极市平台导读本文介绍一篇来自香港科技大学中稿CVPR2021的工 ...
图像反光能被一键去除了？港科大开源RFC，仅用一个操作，强反光也能完美去除｜CVPR2021
本文首发极市平台(微信公众号同名),转载请标明出处. 极市平台是国内专业的计算机视觉开发者平台,原文请戳这里. 代码以及数据集:https://github.com/ChenyangLEI/flash ...
机器人之Cartographer
Cartographer 简介框架介绍可执行节点建图参数 launch文件安装方法(更新中...) 参考: 此文仅作为自身学习使用. 简介整体而言,Local SLAM 部分负责生成较好的子 ...
unity2D小鸟飞过柱子小游戏
(1)启动unity应用程序,在弹出的对话框里单击New project 按钮 (2)我们要得到的效果 (3)首先用户在玩这个游戏的时候小鸟是不断向前飞的,我们知道unity中用户眼睛看到的界面就是摄 ...
柱状图特殊处理，每个柱子颜色不同，图例对应。
series写多个对象,则一个对象就对应一个柱子,注意data数组只能写一个值,不然就会又多个柱子对应一个图例.xAxis,yAxis不用写数据了.
Cartographer使用篇:保存地图
工作环境: ubuntu16.04+ROS(Kinetic)+Cartographer-1.0.0 地图文件 .pbstream cartographer格式地图文件,本质上是一个压缩protobuf ...
反光柱建图venus和我的方案
venus的方式是一种纯反光柱的方案, 网址:https://gitee.com/dustinksi/VEnus 视频地址:https://www.bilibili.com/video/av80266 ...

cartographer如何使用2个反光柱子作为一个landmark

cartographer如何使用2个反光柱子作为一个landmark相关推荐

最新文章

热门文章