再遇咖啡
2022-12-17
qq交流群：742679602
基于图片匹配算法做的slam小车重定位
开源地址：
     https://gitee.com/yongwangzhiqiankai/scanlocation.git
b站视频代码讲解     
     https://b23.tv/HL4AJqr
​重定位的思路：

        我们在显示屏上观看到rviz上的激光点云和地图的可视化数据就能目测出车的坐标是否偏移了，我用rviz上的重定位位工具可以准确的把小车坐标矫正回来；既然人眼能够识别出来的特征，在机器视觉中也应该可以；

所以我的思路的第一步就是把雷达点云转化成图片，因为小车与雷达点云是相对坐标，所以小车在图片上的像素坐标，我们也可以得到，还可以把小车在图片中显示出来，包括车的朝向都能显示出来，姿态角也可以计算出来；

第二步 通过生成点云局部图片与地图图片做模板匹配。匹配得到局部图在全局地图中的像素位置，把得到的像素值与局部图中小车的像素位置值相就得到了小车在全局地图中的像素坐标，然后把小车在全局地图中的像素转成ros地图中的坐标就是小车的准确坐标了，这样就可以重定位了。

如何把点云数据做成局部图：

   首先把雷达点转成map坐标系下的点云，然后再转成图像像素。这样出来的图像就是在全局地图中的像素数据，把这张图中的数据减去最小像素值做图片切割，生成的图片就是局部地图了；

2.项目演示
  动态重定位演示
  静态重定位演示
  静态多线程重点位演示
  融合定位演示





3.opencv 实现图像匹配介绍
	模板匹配
	模板匹配
	模板匹配就是在整个图像区域发现与给定子图像匹配的小块区域。

	在OpenCV中提供了6种匹配度量方法。

	平方差匹配法CV_TM_SQDIFF
	归一化平方差匹配法CV_TM_SQDIFF_NORMED
	相关匹配法CV_TM_CCORR
	归一化相关匹配法CV_TM_CCORR_NORMED
	系数匹配法CV_TM_CCOEFF
	相关系数匹配法CV_TMCCOEFF_NORMED
	随着从简单测量方法（平方差）到更复杂的测量方法（相关系数法），我们可以获得越来越准确的匹配。然而这同时也会以越来越大的计算量为代价。
	对于选取何种方法，针对不同的匹配情况进行对此分析比较，选取更适合自己应用场景同时兼顾速度和精度的最佳方案。 
	注意 :对于方法SQDIFF和SQDIFF_NORMED两种方法来讲，越小的值就有着更高的匹配结果，而其余的方法则是数值越大匹配效果越好。
	


4.代码走读
       前言：
		地图的方向位姿始终是确定的，sacn转的局部地图始终都是平移匹配；
		我们是无法知道真实环境中小车的实际位姿的；
	1.动态重定位
	        就是通过小车自转获得的局部图片与地图匹配，匹配上的小车姿态就是0度；
	  
	  
	  
	2.静态重定位
	        静态重定位可以使用图片旋转的方式或者坐标旋转的方式来做；
		1.用图片旋转没做成功，因为图片转动时旋转中心点会偏移，不好做；
		2.用坐标旋转来做，通过更改ros baselink 中的tf 坐标，后在从tf中取出坐标与雷达点做计算，
		3.也用坐标旋转来做，直接旋转姿态与雷达点做计算，这种好处是我可以同时开360个线程一起计算；


3.整个项目的架构：
	common 模块
	log 日志模块
	图像匹配模块
启动命令：
         roslaunch scanlocation   scan_to_scan_pose.launch
         
         
                用户：
                request_type :

                             initialposeStatic 静态重定位
                             initialposeStaticthread 多线程静态重定位,效率很高
                             initialposeDynamic 动态重定位
                             initiallocation   开始融合定位  会从topic /scanTpngPose 发布坐标
                             initialposeStop 停止重定位
                             savefile        时时保存图片及对应的数据
                             stopSaveFile   停止数据保存

             返回：
                   string result：ok 成功  error 失败
                   string message


          发布融合定位的topic：    /scanTpngPose
          geometry_msgs::PoseStamped  
         
launch:参数
              mappath  地图路径

              laser_index_min 雷达点云最小范围
              laser_index_max 雷达点云最大范围 （sick 的雷达是720个点）

              isshowImage 是否显示图片  true  false 如果显示车的话可能会对结果产生很小的影响。图片上很灰色的点就是车
              showrobotposefig 是否在图片上图片显示车  true  false
              isLocationinitialPosefig  融合定位得到坐标后是否 是否true  false 执行 initialPose 来时时重定位,如果amcl端没有定位计算的坐标做融合，我们可以这么做来时时 initialPose

              turnRepeatNum 满足打分的最低阀值时自动重新匹配的次数; 1 代表再执行一次重定位,2表示2次
              turnRepeatInitialposeTtype 择用 static 静态重定位还是动态 dynamic 多线程 staticthread 重定位做重定位失败后的再次自动重定位,配合 turnRepeatNum参数做

              fazhiLocation 融合定位阀值 达到这个阀值后会从topic  /scanTpngPose中 发布出当前计算好的坐标出来

              fazhimax 重定位阀值 最大匹配分数,达到这个值就会重新定位,不会把剩下没旋转完成的角度任务继续执行下去
              fazhimin  重定位阀值 低于这个阀值会触发再次自动重定位,会配合 turnRepeatNum_  与 turnRepeatInitialposeTtype 两参数完成再次重复重定位

              dynamic_reverse 动态重定位 控制正反转动 1 或 -1
              dynamic_angle 动态重定位需要转一周的角度360度
              dynamic_tolerance 动态重定位 小车转一周的允许误差
              dynamic_speed 动态重定位 需要转一周的转的速度

              rotatAngleStatic 静态重定位 需要转一周的角度360度
              angleStaticxishu 静态重定位 每次递减的角度值