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

3个反光柱的摆放规则：摆成一个直角三角形或者直线或其他特征 ，直角边不同即可，可以设置边的长度来作为附加特征。
原理：检测3个顶点的角度，检测到是直角时可以确定该点是landmark坐标系的原点。然后根据长边和短边来确定坐标系的x轴，这样和激光坐标系的旋转关系就知道了。
好处：不需要标定和其他传感器的外参，激光检测的精度也高，所以整体精度都可以。可以很远距离检测到。
talk is cheap, show your code.

        if (reflector_combination_mode == 3 && reflectors_.size() == 3)
        {
            //log(Info, "reflectors_.size() = " + std::to_string(reflectors_.size()));
            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));
            }
        }

        //主要调试下面这句来约束反光住landmark的构造模式
        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 = 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);

调试运行launch

<launch>
  <arg name="bag_filename" default="/home/~/Downloads/ros_bag//2021-07-07/2021-07-07-10-57-37.bag"/>

  <param name="/use_sim_time" value="true" />

  <param name="robot_description"
    textfile="$(find cartographer_ros)/urdf/backpack_2d_laser_test.urdf" />

  <node name="robot_state_publisher" pkg="robot_state_publisher"
    type="robot_state_publisher" />
 
   <node pkg="laser_reflector_detect" type="laser_reflector_detect_node" name="laser_reflector_detect_node" output="screen" >
       <rosparam file="$(find laser_reflector_detect)/param/laser_reflector_detect.yaml"/>
   </node>

  <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="-d $(find cartographer_ros)/configuration_files/demo_2d_okagv_laser_test.rviz" />
  <node name="playbag" pkg="rosbag" type="play"
      args="--clock -r 1 $(arg bag_filename)" />



</launch>

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