perception/detection/lidar_detector/packages/lidar_naive_l_shape_detect/nodes/lidar_naive_l_shape...

L形障碍物边框计算，车辆边框都为L形状。

LSshapeFilter() 初始化类，设置参数。

updateCpFromPoints(const std::vector& pointcloud_frame_points,autoware_msgs::DetectedObject& object) 更新目标中心点坐标。先获取4个边框点坐标， p1和p3指的是相距最远的两个点，p2指的是一个拐角点，这个拐角点的是根据到p1p3连线距离最远，且p1p2和p2p3连线最接近90度计算的，p4是另外一个拐角点，不过这个拐角点是估算得出的。s1代表p1p2p4组成的三角形的面积，s2代表p2p3p4组成的三角形的面积。中心点cx，cy是由p1和p3插值得到的。

toRightAngleBBox(std::vector& pointcloud_frame_points)  会在P1P2和p2p3组成的角大于90+-6的情况下，对p3和p4进行校正。如果夹角接近90度（+-6），则不需要校正

updateDimentionAndEstimatedAngle(const std::vector& pointcloud_frame_points,             autoware_msgs::DetectedObject& object)  更新目标的尺寸和估计方向角。边框3个点p1p2p3p4，如果p1p2的长度大于p2p3，则p1p2为长，p2p3为宽，目标方向较为p1p2的方向。如果p2p3长度大于p1p2，则p2p3为长，p1p2为宽，方向角为p2p3的夹角。根据边框的方向，计算目标的方向

tf::Matrix3x3 obs_mat;

  obs_mat.setEulerYPR(bb_yaw, 0, 0);

  tf::Quaternion q_tf;

  obs_mat.getRotation(q_tf);

  object.pose.orientation.x = q_tf.getX();

  object.pose.orientation.y = q_tf.getY();

  object.pose.orientation.z = q_tf.getZ();

计算  object.pose.orientation.w = q_tf.getW();

getLShapeBB(const autoware_msgs::DetectedObjectArray& in_object_array,   autoware_msgs::DetectedObjectArray& out_object_array)   获得L形边框主函数。1. 轮询所有聚类出的目标。2. 将点云中第一个点作为参考点，计算整体坐标的偏移。坐标系为：长宽各120米，15点/米，坐标系与图像坐标系相同，左上角为原点。offset_init_pic_point代表当前目标所有点的偏移。3.  遍历输入点云中的每一个点。计算在2坐标系中的坐标，并加2中的偏移量。如果点的x或y坐标大于15×120或者小于0，则continue。4. 计算每个点的斜率，y/x，找出斜率最大和最小的两个点。5. 计算最大最小两点间的斜率和距离。6. 如果斜率和距离都大于某个阈值，则需要计算4个边框点。在点云中随机计算一定数量的点到5中找到的两点组成直线的距离，找出最大的点。这个点就是算法中的p2。p4用p1,p2,p3计算。7. 如果斜率和距离某一个小于阈值，则调用cv::minAreaRect计算包含所有点的最小四边形。8. 调用updateCpFromPoints() 利用4个边框点，重新计算目标的中心点。9. toRightAngleBBox() 如果p1p2，p2p3夹角大于阈值，则需要校正p2,p3两个点。也就是检查边框点是否需要更新。10. 用9更新后的边框点，计算目标的方向，存入到目标的位姿中。