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

107 篇文章 76 订阅

2个反光柱的摆放要求：靠着墙摆，因为我们只能从一侧看到2个反光柱，从两侧都看到至少需要3个反光柱来定位。
landmark检测原理：因为我们是从一侧看到2个反光柱，所以看到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 out
if (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 origin
double 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 origin
double 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 origin
double 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 origin
double 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 points
geometry_msgs::PointStamped point;

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.landmarks.push_back(reflector_LandMarkEntry);

#ifdef VISUAL_RVIZ
visualization_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.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_poses_list.markers.push_back(landmark_item);
#endif
}
}

• 3
点赞
• 23
收藏
• 打赏
• 7
评论
09-11
10-19 598
07-27 343
03-29 5655
12-05 501
09-04 2913
07-20 305
06-28 885
10-08 528

### “相关推荐”对你有帮助么？

• 非常没帮助
• 没帮助
• 一般
• 有帮助
• 非常有帮助

¥2 ¥4 ¥6 ¥10 ¥20

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、C币套餐、付费专栏及课程。