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

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


下一篇:开源自主导航小车MickX4(七)cartographer 室外3D建图