RatSLAM边用边学之经验地图

 em在设计的时候分挺多层的，很多的exp和link分别都存在vector中。整体模式是em_map包括经验exp和链接link的内容。exp之间是没有关联的，通过link链接，一个link中包含当前节点和后面（所有？）节点的链接信息。所以一个exp对应的link就有一个vector。exp和link之间的关联，是通过同一个id号的。




在园子里看到的扫盲下视觉定位的工作原理：
3.2 基于区域的匹配

　　本文采用的就是基于区域的相关匹配方法。它是把一幅图像中的某一点的灰度领域作为模板，在另一幅图像中搜索具有相同（或相似）灰度值分布的对应 点领域，从而实现两幅图像的匹配。在基于区域相关的算法中，要匹配的元素是固定尺寸的图像窗口，相似准则是两幅图像中窗口间的相关性度量。当搜索区域中的 元素使相似性准则最大化时，则认为元素是匹配的。

　　定义P (i, j) P 是模板图像中一点，取以P (i, j) P 为中心的某一邻域作为相关窗口K ，大小为(2w +1)，假设K 在原始图中，水平方向平移Δu ，垂直方向平移Δu 后，K 所覆盖下的那块搜索区域叫做子图S k ，若K 和S k 相同，则它们的差为零，否则不为零。由此定义K 和S k 的相关函数为：

　　当D(K, S k )达到最小，K 与S k 达到最佳匹配。

4. 空间坐标的获取

　　由一幅图像得到的信息是二维信息，程序中使用的坐标是以像素为单位的，机器人在空间运动需要将图像的信息换算成三维空间坐标。其计算过程如下：

　　（1）以工件上圆孔的圆心为机器人定位基准，A(X ,Y, Z)圆心的世界坐标。当圆心与视觉图像的中心重合时，机器人定位完成。

　　（2）标定摄像机，得到投影矩阵ce M ，即图像中两个像素间的距离与世界坐标系的转换关系。

　　（3）摄像机拍摄图像后，经过特征识别得到圆心在图像中坐标a(x, y)，计算出与图像中心的偏移量Δx、Δy。

　　（4）以A(X ,Y, Z)为基准，按照下式计算机器人末端的世界坐标B(X ',Y ', Z')：其中，Mc是摄像机与机器人末端的坐标转换参数。

　　注意，本文使用单目视觉，所以这里假设 Z 坐标不变；如果使用双目视觉，就可以通过立体视觉计算出Z’。


（3）运用DH 法计算各关节的转角θi ：