基于激光雷达实现三边定位算法开发记录（七）——反光柱匹配

22年暑假实习过程中第一个开发任务，基于激光雷达实现三边定位功能

开发平台为ubuntu 18.04 + ros melodic

日期：2022.7.27

本次实现：在能对拟定圆心数据进行保存和读取之后，本次将用上一次保存的数据作为目标数据，再将激光雷达换位置后的当前数据与其进行匹配，从当前的五个反光柱中找出需要的三个目标反光柱。匹配的原理是将预先录入系统的反光柱之间的边长一一去和当前能识别到的反光柱之间的边长匹配，识别出目标反光柱。并返回目标反光柱的坐标以便后期进行三边定位算法实现。

（一）代码编写

编写反光柱匹配函数，传入当前识别到的反光柱数据与保存的目标反光柱坐标数据，分别求点与点之间的距离关系，再将两组距离去进行匹配，当距离匹配成功后再将这段距离两端的坐标记录进一个数组，当数组内该坐标出现且仅出现两次时，判断为目标反光柱，并进行标注。最后返回筛选出来的目标反光柱坐标。

1. //反光柱匹配

2. void reflectorMatching(std::vector< Point > circle,std::vector< std::pair< float, float > > preservation,std::vector< std::pair< float, float > > &matching_coordinates){

3. std ::vector<float> distance_coordinate; //保存的各个坐标之间的距离

4. std ::vector<Distance> distance_circle; //当前各个反光柱之间的距离

5. std ::vector<float> matching; //匹配的反光柱编号

6. std::pair< float, float > matching_circle; //匹配的反光柱坐标

7. std::set<long> matching_num; //

8. Distance dis_circle;

11. float x_ij;

12. float y_ij;

13. float distance_ij;

14. distance_coordinate.clear();

15. distance_circle.clear();

17. for(long i = 1; i < preservation.size() ; i++){ //遍历记录的坐标距离

18. for(long j = 0;j < i;j++){

19. if(i == j){

20. continue;

21. } else {

22. x_ij = fabs(preservation[j].first - preservation[i].first);

23. y_ij = fabs(preservation[j].second - preservation[i].second);

24. distance_ij = sqrt(pow(x_ij, 2) + pow(y_ij, 2));

25. distance_coordinate.push_back(distance_ij);

26. }

27. }

28. }

30. for(long i = 1; i < circle.size() ; i++){ //遍历当前各个反光柱圆心的坐标距离

31. for(long j = 0;j < i;j++){

32. if(i == j){

33. continue;

34. } else {

35. x_ij = fabs(circle[j].x - circle[i].x);

36. y_ij = fabs(circle[j].y - circle[i].y);

37. distance_ij = sqrt(pow(x_ij, 2) + pow(y_ij, 2));

39. dis_circle.distance = distance_ij;

40. dis_circle.one = i;

41. dis_circle.two = j;

42. distance_circle.push_back(dis_circle);

43. }

44. }

45. }

46. for(long i = 0;i < distance_circle.size();i++){ //匹配记录的和当前的坐标距离

47. // std::cout << "当前匹配距离：" << distance_circle[i].distance << std:: endl;

48. for(long j = 0;j < distance_coordinate.size();j++){

49. // std::cout << "预设匹配距离：" << distance_coordinate[j]<< std:: endl;

50. if(abs(distance_coordinate[j] - distance_circle[i].distance) < 0.02) { //匹配误差 0.02 m

51. // std::cout << "能匹配上" << j <<std:: endl;

52. matching.push_back(distance_circle[i].one);

53. matching.push_back(distance_circle[i].two);

54. }

55. }

56. }

58. for(int i=0;i<matching.size();i++) { //去除重复编号

59. matching_num.insert(matching[i]);

60. }

62. for(std::set<long>::iterator it=matching_num.begin();it!=matching_num.end();it++) {

63. std::cout << "匹配成功，编号：" << *it<< std::endl;

64. std::cout << "x轴："<<circle[*it].x << "y轴："<<circle[*it].y << std::endl;

65. if(count(matching.begin(), matching.end(), *it) == 2){ //当单个反光柱被作为顶点时编号会出现两次 才匹配上

66. set_matching_marker_property(circle[*it].x ,circle[*it].y,*it); //发布匹配标记

68. matching_circle.first = circle[*it].x ;

69. matching_circle.second = circle[*it].x ;

70. matching_coordinates.push_back(matching_circle); //记录匹配上的反光柱坐标

72. } else {

73. ROS_WARN("Match failed!!!");

74. }

75. }

76. }

新增匹配标记函数，与反光柱标记函数类似，区别在于颜色与透明度，最后的持续时间新加为0.5s，防止匹配标记停留。

1. // 设置匹配标记并发布

2. void set_matching_marker_property(float mx,float my,long n){

3. /*决定从哪个视图可以看到标记r*/

4. marker_.header.frame_id = "laser_frame";

5. marker_.ns = "sign2";

6. marker_.id = n;

7. //设置标记类型：圆柱体

8. marker_.type = visualization_msgs::Marker::CYLINDER;

10. //设置标记坐标

11. marker_.pose.position.x = mx;

12. marker_.pose.position.y = my;

13. marker_.pose.position.z = 0;

15. //设置标记尺寸

16. marker_.scale.x = 0.2; //m

17. marker_.scale.y = 0.2;

18. marker_.scale.z = 0.2;

20. //设置标记颜色

21. marker_.color.a = 0.5; // Don't forget to set the alpha!

22. marker_.color.r = 0.0;

23. marker_.color.g = 1.0;

24. marker_.color.b = 0.0;

26. //设置标记动作

27. marker_.action = visualization_msgs::Marker::ADD;

28. marker_.lifetime = ros::Duration(0.5); //0.5s后消失，输入0时为不消失

30. Publish();//发布

31. };

修改处理激光信息函数。

1. //处理激光信息

2. void scan_CB(sensor_msgs::LaserScan scan) {

3. std::vector< Outline > outlines; //

4. std::vector< Point > circle; //当前反光柱数据

5. std::vector< std::pair< float, float > > preservation_coordinates; //保存的反光柱坐标

6. std::vector< std::pair< float, float > > matching_coordinates; //匹配的反光柱坐标

8. std::string fileaddress = "/home/ding/test_ws/src/HAL/trilateration/src/CircleData.txt"; //反光柱坐标数据文件地址

10. bool fileoperation_state = false; //true; //文件读写标志为，true为写，false为读

12. for (int i = 0; i < 5; i++) {

13. circle = findCircle(scan); //获取各个反光柱圆心

14. }

15. fileOperation(fileaddress,circle,preservation_coordinates,fileoperation_state); //对获取的反光柱数据进行读写文件处理

16. if(!fileoperation_state){

17. reflectorMatching(circle,preservation_coordinates,matching_coordinates); //读文件状态下 保存的数据与当前的数据进行匹配

18. }

19. }

（二）运行结果

匹配成功—五选三

匹配失败—不生成绿色匹配标记

（三）问题和下一目标

问题：目前仅能简单识别出反光柱，当目前反光柱数量增加时是否会出现错误还不知道，且当反光柱距离较远时，可能会出现匹配不上的情况。

目标：根据最后拿到的目标反光柱坐标进行三边定位和坐标变换，求出激光雷达（即机器人）在全局之中的坐标。

基于激光雷达实现三边定位算法开发记录（七）——反光柱匹配_基于反光柱的匹配-CSDN博客