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 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.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_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.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
}
}