python实现Nao机器人的单目测距

本文实例为大家分享了python实现Nao机器人单目测距的具体代码,供大家参考,具体内容如下

此代码适于用做对Nao机器人做视觉识别和测距实验,只提供关键代码部分,尝试利用cv2去优化代码会更加简洁哟!

此代码的主要功能:

1.初始姿态下,通过更换摄像头和转头去寻找目标
2.通过颜色阈值识别目标,计算目标与Nao的距离和角度

可以扩展功能:

1.在运动过程中对方向和距离进行多次测量和校正,提高准确度
2.找到目标后,通过对目标的测量,选择使用哪个脚去踢目标

#!/usr/bin/python2.7
#-*- encoding: UTF-8 -*-
import vision_definitions
#----------------------单目测距--------------------------------
#***********************************************
#@函数名:   DistAndDirect_cal(cxnum,rynum,colsum,rowsum,Head_angle,cameraID)
#@参数:     (cxnum,rynum)是通过图像识别得到球心的像素点坐标
#           (colsum,rowsum)是图片总大小:640*480
#            cameraID=0,取上摄像头;cameraID=1,取下摄像头
#@返回值:   无
#@功能说明: 采用机器人的下摄像头进行测量球离机器人的相关角度与距离
def DistAndDirect_cal(cxnum,rynum,colsum,rowsum,Head_angle,cameraID):
    distx=-(cxnum-colsum/2)
    disty=rynum-rowsu/2
    print distx,disty
    Picture_angle=disty*47.64/480

    if cameraID ==0:
        h=0.62
        Camera_angle=12
    else:
        h=0.57
        Camera_angle=38
    #Head_angle[0]机器人仰俯角度
    Total_angle=math.pi*(Picture_angle+Camera_angle)/180+Head_angle[0]
    d1=h/math.tan(Total_angle)

    alpha=math.pi*(distx*60.92/640)/180
    d2=d1/math.cos(alpha)
    #Head_angle[1]机器人左右角度
    Forward_Distance=d2*math.cos(alpha+Head_angle[1])
    Sideward_Distance=-d2*math.sin(alpha+Head_angle[1])
#***********************************************

#@函数名:   GetNaoImage(IP,PORT,cameraID)
#@参数:     略
#@返回值:   无
#@功能说明: 采调用机器人内置摄像头控制模块,对当前场景进行拍摄并保持。
#           由于球距离机器人约小于0.6m时,机器人额头摄像头无法看到,
#           所以需要变换摄像头,cameraID=0,取上摄像头;
#           cameraID=1,取下摄像头
def Get NaoImage(IP,PORT,cameraID):
    camProxy=ALProxy("ALVideoDevice",IP,PORT)
    resolition=2 #VGA格式640*480
    colorSpace=11#RGB

    #选择并启用摄像头
    camProxy.setParam(vision_definitions.kCameraSelectID,cameraID)
    videoClient=camProxy.subscribe("python_client",resolition,colorSpace,5)

    #获取摄像机图像。
    #image [6]包含以ASCII字符数组形式传递的图像数据。
    naoImage=camProxy.getImageRemote(videoClient)

    camProxy.unsubscribe(videoClient)
    #获取图像大小和像素阵列。
    imageWidth=naoImage[0]
    imageHeight=naoImage[1]
    array=naoImage[6]
    #从我们的像素阵列创建一个PIL图像。
    im=Image.fromstring("RGB",(imageWidth,imageHeight),array)
    #保存图像。
    im.save("temp.jpg","JPEG")

#***********************************************
#@函数名:   findColorPattern(img,pattern)
#@参数:     略
#@返回值:   无
#@功能说明:  将RGB图像转化为二值图:此方法用的是cv,可以尝试用cv2代码会更加简洁
def  findColorPattern(img,pattern):
    channels=[None,None,None]
    channels[0]=cv.CreateImage(cv.GetSize(img),8,1)
    channels[1]=cv.CreateImage(cv.GetSize(img),8,1)
    channels[2]=cv.CreateImage(cv.GetSize(img),8,1)
    ch0=cv.CreateImage(cv.GetSize(img),8,1)
    ch1=cv.CreateImage(cv.GetSize(img),8,1)
    ch2=cv.CreateImage(cv.GetSize(img),8,1)
    cv.Split(img,ch0,ch1,ch2,None)
    dest=[None,None,None,None]
    dest[0]=cv.CreateImage(cv.GetSize(img),8,1)
    dest[1]=cv.CreateImage(cv.GetSize(img),8,1)
    dest[2]=cv.CreateImage(cv.GetSize(img),8,1)
    dest[3]=cv.CreateImage(cv.GetSize(img),8,1)
    cv.Smooth(ch0,channels[0],cv.CV_GAUSSIAN,3,3,0)
    cv.Smooth(ch1,channels[1],cv.CV_GAUSSIAN,3,3,0)
    cv.Smooth(ch2,channels[2],cv.CV_GAUSSIAN,3,3,0)
    for i in range(3):
        k=2-i
        lower=pattern[k]-75#设置阈值
        upper=pattern[k]+75
        cv.InRangeS(channels[i],lower,upper,dest[i])

    cv.And(dest[0],dest[1],dest[3])
    temp=cv.CreateImage(cv.GetSize(img),8,1)
    cv.And(dest[2],dest[3],temp)
    '''
    cv.NameWindow("result",cv.CV_WINDOW_AUTOSIZE)
    cv.ShowImage("result",temp)
    cv.WaitKey(0)
    '''
    return temp

#***********************************************
#@函数名:   xyProject(matrix,imgaesize)
#@参数:     matrix
#           imgaesize
#@返回值:   无
#@功能说明: 利用二值图,计算球的像素坐标。其原理是:遍历各行各列
#           像素的数值的和,最大的组合即为球心坐标
def xyProject(matrix,imagesize):
    #声明一个数据类型为8位型单通道的imagessize[1]*1/1*imagessize[0]矩阵(初始值为 0)。
    colmask=cv.CreateMat(imagessize[1],1,cv.CV_8UC1)
    rowmask=cv.CreateMat(1,imagessize[0],cv.CV_8UC1)
    cv.Set(colmask,1)
    cv.Set(rowmask,1)

    colsum=[]
    for i in range(imagesize[0]):
        col=cv.GetCol(matrix,i)
        #计算向量点积
        a=cv.DotProduct(colmask,col)
        colsum.append(a)

    rowsum=[]
    for i in range(imagesize[1]):
        row=cv.GetRow(matrix,i)
        a=cv.DotProduct(rowmask,row)
        rowsum.append(a)

    return(colsum,rowsum)#得到各行各列“1”值的和

def crMax(colsum,rowsum):
    cx=max(colsum)
    ry=max(rowsum)
    for i in range(len(colsum)):
        if colsum[i]==cx:
            cxnum=i
    for i in range(len(rowsum)):
        if rowsum[i]==ry:
            rynum=i
    return(cxnum,rynum)
#***********************************************
#@函数名:  GetHeadAngles(robotIP,PORT)
#@参数:    略
#@返回值:   无
#@功能说明:
def GetHeadAngles(robotIP,PORT):
    motionProxy=ALProxy("ALMotion",robotIP,PORT)
    names=["HeadPitch","HeadYaw"]
    useSensors=1
    sensorAngles=motionProxy.getAngles(names,useSensors)
    return sensorAngles
#***********************************************
#@函数名:  SetHeadAngles(robotIP,PORT,angles)
#@参数:    略
#@返回值:   无
#@功能说明:
def SetHeadAngles(robotIP,PORT,angles):
    motionProxy=ALProxy("ALMotion",robotIP,PORT)
    motionProxy.setStiffnesses("Head",1.0)

    names=["HeadPitch","HeadYaw"]
    fractionMaxSpeed=0.2
    motionProxy.setAngles(names,angles,fractionMaxSpeed)

    time.sleep(2.0)
    motionProxy.setStiffnesses("Head",0.0)

#***********************************************
#@函数名:   Capture_Picture(IP,PORT,cameraID,angles,pattern_colors)
#@参数:     angles
#           pattern_colors
#@返回值:   无
#@功能说明: 将上面的一系列函数整合起来

def Capture_Picture(IP,PORT,cameraID,angles,pattern_colors):
    SetHeadAngles(IP,PORT,angles)
    GetNaoImage(IP,PORT,cameraID)
    image=cv.LoadImage("temp.jpg")
    imagesize=cv.GetSize(image) #返回数值,两个元素分别为列数和行数
    matrix=findColorPattern(image,pattern_colors)
    (colsum,rowsum)=xyProject(matrix,imagesize)
    (cxnum,rynum)=crMax(colsum,rowsum)
    cv.SaveImage("result.jpg",matrix)

    return (cxnum,rynum,colsum,rowsum)

#***********************************************
#@函数名:   Target_Detect_and_Distance(IP,PORT)
#@参数:
#@返回值:   无
#@功能说明: 当上摄像头无法找到球时,切换到下摄像头,然后在左转右转。
#       在这个过程中,如果发现目标,则计算距离并输出距离
#       若始终未找到目标,则输出距离为0。

def Target_Detect_and_Distance(IP,PORT):
    pattern_colors=(255,150,50)
    cameraID=0# 默认上摄像头
    angles=[0,0]
    (cxnum,rynum,colsum,rowsum)=Capture_Picture(IP,PORT,cameraID,angles)

    if(cxnum,rynum)==(639,479):
        cameraID=1
        (cxnum,rynum,colsum,rowsum)=Capture_Picture(IP,PORT,cameraID,angles)
    if(cxnum,rynum)==(639,479):
        cameraID=0
        angles=[0.0.7]
        (cxnum,rynum,colsum,rowsum)=Capture_Picture(IP,PORT,cameraID,angles)
    if(cxnum,rynum)==(639,479):
        cameraID=0
        angles=[0,-0.7]
        (cxnum,rynum,colsum,rowsum)=Capture_Picture(IP,PORT,cameraID,angles)
    HeadAngles-GetHeadAngles(IP,PORT)
    ###############
    (Forward_Distance,Sideward_Distance)=DistAndDirect_cal(cxnum,rynum,colsum,rowsum,Head_angle,cameraID)
    if(cxnum,rynum)==(639,479):
        (Forward_Distance,Sideward_Distance)=(0,0)
    print "Forward_Distance=",Forward_Distance,"meters"
    print "Sideward_Distance="+Sideward_Distance+"meters"

#***********************************************
#@函数名:   Target_Detect_and_Distance(IP,PORT)
#@参数:
#@返回值:   无
#@功能说明: 当找到球后,可能会存在一定的误差。
#           因此需要判断球位于机器人前方的哪一侧,再来确定用哪只脚踢球

def Final_See(robotIP,PORT):
    pattern_colors=(255,150,50)
    angles=[0.5,0]
    SetHeadAngles(robotIP,PORT,angles)

    cameraID=1

    GetNaoImage(robotIP,PORT,cameraID)
    image=cv.LoadImage("temp.jpg")
    imagesize=cv.GetNaoImage(image)

    matrix=findColorPattern(image,pattern_colors)

    (colsum,rowsum)=xyProject(matrix,imgaesize)
    (cxnum,rynum)=crMax(colsum,rowsum)
    cv.SaveImage("result.jpg",matrix)

    HeadAngles=GetHeadAngles(robotIP,PORT)
    #########################
    (Forward_Distance,Sideward_Distance)=DistAndDirect_cal(cxnum,rynum,colsum,rowsum,Head_angle,cameraID)

    if cxnum<len(colsum)/2:
        side=0#左脚
    else:
        side=1#右脚
    print "side=",side
    print "last distance=",Forward_Distance
    return (side,Forward_Distance)

以上就是本文的全部内容,希望对大家的学习有所帮助,也希望大家多多支持我们。

(0)

相关推荐

  • python程序控制NAO机器人行走

    最近重新学习nao的官方文档,写点简单的程序回顾一下.主要是用python调用api,写下来保存着. '''Walk:small example to make nao walk''' import sys import motion import time from naoqi import ALProxy def StiffnessOn(proxy): #we use the 'body' to signify the collection of all joints pName="Body

  • python实现nao机器人手臂动作控制

    本文实例为大家分享了python实现nao机器人手臂动作的具体代码,供大家参考,具体内容如下 这些天依然在看nao公司文档的东西,把读过的代码顺手敲了出来.代码依然很简单,但是为什么我要写博客呢?这其中有很大的原因在于,代码是死的,可是读着读着就感觉代码活了,而且,每次读都会有不同的感受.咱就直接看正题吧. #-*-encoding:UTF-8-*- import sys import motion import almath from naoqi import ALProxy def Stif

  • python距离测量的方法

    之所以写这个,其实就是希望能对距离有一些概念,当然这个也是很基础的,不过千里之行始于足下嘛,各种路径算法,比如a*什么的都会用到这个 距离测量有三种方式 1.欧式距离,这个是最常用的距离测量方式 ((x1-x2)^2 + (y1-y2)^2)^0.5 得到一个原型区域的距离 #设起始坐标是原点,即(0,0) y_result = [] for y in range(10,-10,-1): x_result = [] for x in range(-10,10,1): #((0-x)**2 + (

  • python实现nao机器人身体躯干和腿部动作操作

    本文实例为大家分享了python实现nao机器人身体躯干和腿部动作的具体代码,供大家参考,具体内容如下 跟上一篇类似,代码没什么难度,可以进行扩展. #-*-encoding:UTF-8-*- '''control nao's left foot, cartesian control:torso and foot trajectories ''' import sys import motion from naoqi import ALProxy def StiffnessOn(proxy):

  • python控制nao机器人身体动作实例详解

    本文实例为大家分享了python控制nao机器人身体动作的具体代码,供大家参考,具体内容如下 今天读的代码,顺便写了出来,与文档的对比,差不多. import sys import motion import almath import naoqi from ALProxy def StiffnessOn(proxy): pName="Body" pStiffnessLists pTime=1.0 proxy.stiffnessInterpolation(pName,pStiffnes

  • python实现Nao机器人的单目测距

    本文实例为大家分享了python实现Nao机器人单目测距的具体代码,供大家参考,具体内容如下 此代码适于用做对Nao机器人做视觉识别和测距实验,只提供关键代码部分,尝试利用cv2去优化代码会更加简洁哟! 此代码的主要功能: 1.初始姿态下,通过更换摄像头和转头去寻找目标 2.通过颜色阈值识别目标,计算目标与Nao的距离和角度 可以扩展功能: 1.在运动过程中对方向和距离进行多次测量和校正,提高准确度 2.找到目标后,通过对目标的测量,选择使用哪个脚去踢目标 #!/usr/bin/python2.

  • 基于python opencv单目相机标定的示例代码

    相机固定不动,通过标定版改动不同方位的位姿进行抓拍 import cv2 camera=cv2.VideoCapture(1) i = 0 while 1: (grabbed, img) = camera.read() cv2.imshow('img',img) if cv2.waitKey(1) & 0xFF == ord('j'): # 按j保存一张图片 i += 1 u = str(i) firename=str('./img'+u+'.jpg') cv2.imwrite(firename

  • 如何基于python实现单目三维重建详解

    目录 一.单目三维重建概述 二.实现过程 (1)相机的标定 (2)图像特征提取及匹配 (3)三维重建 三.结论 四.代码 总结 一.单目三维重建概述 客观世界的物体是三维的,而我们用摄像机获取的图像是二维的,但是我们可以通过二维图像感知目标的三维信息.三维重建技术是以一定的方式处理图像进而得到计算机能够识别的三维信息,由此对目标进行分析.而单目三维重建则是根据单个摄像头的运动来模拟双目视觉,从而获得物体在空间中的三维视觉信息,其中,单目即指单个摄像头. 二.实现过程 在对物体进行单目三维重建的过

  • OpenCV实现单目摄像头对图像目标测距

    使用opencv对单目摄像头中的目标实现测量距离(python实现),供大家参考,具体内容如下 1.方法介绍: 根据相似三角形的方法: F = P×D / W , 其中W为物体的实际宽度,D为物体平面与相机平面的距离,照片上物体的像素宽度为P,相机焦距为FD = F×W / P ,这样知道相机的内参焦距就可以求得物体平面与相机平面的距离D 2.代码: import cv2 import imutils import numpy as np from imutils import paths #

  • OpenCV实现单目尺寸估计的案例详解

    一个摄像头无法获取深度信息,两个摄像头双目标定可以实现双目测距. 但是我现在只有一个摄像头,如果想实现测量尺寸,我的思路:选一张固定尺寸的背景,例如一张A4纸,从图像中提取A4纸并进行透视变换进行图像矫正,A4纸的尺寸我可以确定,那么也可以确定图像中的物体长宽信息(高度忽略不计的情况,例如:卡片).当摄像头距离目标物距离L,此时像素所占面积为S,那么理论上来说,目标物图像变化后的面积为S1,则距离L1=(L/S)*S1,假定目标物上面贴有很多个面积为1平方厘米的正方形贴纸,那么获取四个角点和四条

  • OpenCV NAO机器人辅助捡球丢球流程分析

    目录 1. 概述 2. 相关技术 2.1 NAO机器人 2.2 Choregraphe 3. 总体设计与详细设计 3.1 系统模块划分 3.2 主要功能模块 4 视频展示 1. 概述 NAO机器人身高58cm,体重5.4kg.主要硬件包括CPU.主板.扬声器.话筒.红外线.相机.超声波(声呐).传感器.电机.语音合成器.陀螺仪等.实验室中NAO机器人的型号为NAOH25,其构造如下: NAO机器人的操作系统为Gentoo Linux,它支持Windows.Linux.Mac OS等操作系统的远程

随机推荐