localize by triangle note

1.当拟合结果不为1时，取和上次相近的作为结果
2.python画三角拟合图bug

(3.减小三角拟合波动：对激光雷达数据进行滤波等处理）
(4.在计算三角起始和结束位置时，添加用距离值过滤)

theta:三角角平分线与小车坐标系x轴的夹角，三角位于x轴右侧时夹角为负值，左侧为正值，单位弧度 （衡量小车中心在三角角平分线的左右侧）
beta:三角顶点与小车坐标系x轴的夹角，三角位于x轴右侧时夹角为负值，左侧为正值，单位弧度 （衡量三角顶点在小车头的左右侧）
alpha = triangle_angle_with_axisX - theta：用来度量小车与最终目标方向的偏差程度
rho：是三角到小车中心的距离值

转角 + 车头控制
转角 + 车中心在三角角平分线上 + 转角 + 车头控制
转角 + 车中心在三角角平分线上 + 转角 + 车头控制（车中心检测）
车中心控制法：转角 + 车中心在三角角平分线上 + 转角

线速度为0时，角速度<0.001小车不动，所以没有意义；线速度为0.002时，角速度为0.0005小车有微小转角

TransMsgToCart(): laser_cart_points(0, i) = X0 + range * cos(q);//雷达倒立时添加负号

//：雷达正放和倒放时line1和line2,相反。正：从左向右是line2,line1；倒立：从左向右是line1,line2

//计算两条线的夹角及交点
/*设直线l1、l2的斜率存在,分别为k1、k2,且夹角不是90度,l1到l2的转向角为θ,则tanθ=(k2- k1）/(1+ k1k2）
  l1与l2的夹角为θ,则tanθ=∣(k2- k1）/(1+ k1k2）∣.  直线的斜率公式：k=（y2-y1）/（x2-x1）
  注意：两直线的夹角指的是两直线所成的小于90°的锐角,显然夹角公式中的“角”并不都是两直线的夹角*/

5.从tf获得欧拉角roll, pitch, yaw

static tf::TransformListener tf_listener;
    tf::StampedTransform laser_transform;
    try
    {
      ros::Time now = ros::Time::now();
      tf_listener.waitForTransform (frame_id_laser_, now, frame_id_laser_, now, frame_id_agv_, ros::Duration ( 0.05 ) );
      tf_listener.lookupTransform(frame_id_agv_, frame_id_laser_, now, laser_transform);
    }
    catch (tf::TransformException ex)
    {
      ROS_WARN("handeld tf laser to agv error: %s\n",ex.what());
      return;
    }

double roll, pitch, yaw;
    laser_transform.getBasis().getEulerYPR(yaw, pitch, roll);

6.关于三角正倒立问题

1)，不管激光雷达位置如何旋转变化，三角相对激光雷达的位置是固定的，因此把激光点从极坐标转换到激光雷达直角坐标系时不需要考虑雷达的旋转

2)，使用python观察激光点从极坐标转换到激光雷达直角坐标系时，注意雷达的坐标系（雷达的旋转），因为python画的直角坐标系是右手坐标系

3)，三角位置的计算可在激光雷达坐标系下进行，计算出三角pose后可将该pose转换到机器人坐标系下使用（此时会用到激光雷达的旋转）