利用单目视觉做NAO机器人摄像头坐标转换

趁着比赛将近，总结一下对于NAO机器人摄像头坐标转换问题的理解。

一.问题的来由：

为了让NAO机器人识别某个物体，然后走到物体面前一段距离的位置停止，需要知道NAO和物体的距离和角度。通过查找aldebaran documentation可知用函数getTargetPositon（）可以得到某些特定的物体的坐标。

http://doc.aldebaran.com/2-1/naoqi/trackers/altracker.html#tracking-targets

但是对于一般的物体而言这个并没有什么卵用。。

二.解决办法  

    用简单的单目测距原理和相似三角形原理作一个坐标变换：

    设以摄像头为坐标原点的坐标系为A（原点：摄像头镜头，X轴正前方，Y轴左前方）

       以摄像头焦平面为坐标原点的坐标系为B（原点：2D图像中心（640x480的图就是（320，240）啦），注意这个X轴和Y轴方向如图所示：

图中的角60.97°和47.64°分别是NAO机器人的水平张角和垂直张角。

由三角函数变换：alpha=((320-x)/640)*60.97*pi/180

     beta=((y-240)/480)*47.64*pi/180

由NAO提供的API，getAngles（）参数设置成HeadYaw和HeadPitch获得头部姿态，假设是Rotation1和Rotation2，则可以得到校正后的alpha'和beta':

alpha'=alpha+Rotation1

beta'=beta+Rotation2

然后通过AngleInterpolation()将头部转到目标物中间，理想状态下应该物体重心点正好在图像的中心（320,240)

三.接着就可以测量距离啦

通过以上变换是为了让机器人的摄像头正好对准物体中心，这样构造出来的计算距离的模型会是一个平面三角形，简化了计算。

V5的NAO身高H总共是459.59mm，假设物体长度L，则有：

tan(phi+theta)=(H-L/2)/S

    其中theta是下摄像头和水平方向夹角，为39.7°  

    phi可以由函数getangle（）带入参数"HeadPitch"得到

    将已知条件代入即可得到距离S

以上是一种简单的获得物体坐标的方式，其优点是写代码简单快捷，缺点是没有摄像头标定精度较低，且应用范围较小。可作为替代的还有双目视觉和采用深度摄像头获得三维坐标的方案，等有空再更新。