一种基于双目视觉目标检测的家居环境下门定位方法与流程

本发明涉及机器人视觉领域，是一种实时的目标定位方法，尤其是一种基于双目视觉目标检测的家居环境下门定位方法。

背景技术：

近年来，机器人技术作为高新科技，得到了不断地发展，在家庭、办公室、医院和工厂等得到广泛采用。从生产车间用于智能装配的工业机器人到用于公共服务、家庭服务的智能服务机器人，机器人已经越来越贴近并进入我们的工作和生活中。目前新兴的服务机器人企业和服务机器人应用市场也不断地增加，所以各式各样的娱乐、救援、监视机器人也逐渐渗透进入我们生活的方方面面。随着对服务机器人智能程度需求的愈来愈高，对以人工识别技术、人机交互技术、自主巡逻技术和智能控制技术等为代表的智能技术要求也愈来愈高。对于家庭服务机器人，尤其是监视机器人，室内导航和自主巡逻通常是必要的功能。因此对于该类机器人最大的挑战是在对周边环境进行有限了解的基础上，可自主的执行越来越多的复杂任务。其中门对机器人在家居环境中地图构建、室内导航、自主巡逻具有极其重要的意义。其原因如下：

1.门在家居环境的位置是相对不变的；

2.门是连接一个房间与另一个房间的唯一通道；

3.门的宽度和形状具有统一的标准。

但是一些家庭服务型机器人因其集成功能较多，体积相对庞大，但家居环境下的门相对较小。实现对门定位，并顺利通过门，成为机器人技术发展的难点。所以实现对家居环境下门的定位，对于机器人技术和产业的发展具有十分重要的意义。

国际上有很多科研机构和厂商都在研究家居环境下门的定位问题，通过采用摄像头、声呐传感器、激光雷达传感器等来实现传感器信息或多传感器信息融合，并提出了一系列的解决方案，如：

1.在门两侧粘贴标签，并通过单目视觉对标签的定位进而实现对门的定位；

2.采用声呐传感器或激光雷达传感器探测机器人前进方向上是否有深度突变区，并结合已知地图信息实现对开着的门定位；

3.采用RGB-D传感器，通过对门的特征进行提取，并结合视野中各物体的深度信息实现对门的定位；

4.采用单目视觉，通过提取图像中线特征，进一步匹配门线特征的各种组合情况，实现对门的定位。

但上述方法都有一定的缺陷。首先，声呐传感器、激光雷达传感器价格很高，RGB-D传感器的价格也相对不低，所以带有上述传感器的机器人很难推广到各个经济阶层的家庭中；而且此方案也存在不足，不能对关着的门实现定位。其次，上述方法1和方法4虽然成本比较低，但是方法1需要对家庭环境进行修改，并且操作较为繁琐，难以被用户所接受；方法4需要机器人需要离门有一定距离才可实现对门的定位，否则门与地的界限会位于摄像头的死角范围内，在实际应用中受到一定的局限。

技术实现要素：

本发明提出一种基于双目视觉目标检测的家居环境下门定位方法，该方法通过采用双目视觉目标检测实现对家居环境下门的定位，通过高效的算法有效的解决了采用传感器、激光雷达传感器等传感器方案价格昂贵的缺陷，降低了硬件成本；而且，本方法不需要在门上增加多余的标志物，在实际操作中更为简单方便；同时，本方法只需要门的两侧边框同时出现在双目视野中便可实现对门的定位，实际使用更加的灵活；本方法还可以实现对家居环境下不同状态的门实现定位，比如关闭、半开、全开的门，具有更加的广泛应用前景。

本发明的技术方案提供了一种基于双目视觉目标检测的家居环境下门定位方法，包括以下步骤：

S101、将双目视觉摄像机(左摄像机和右摄像机)置于距离地面一定高度的支架上，水平放置，接收来自两路双目视觉摄像机的图像I_L、I_R；

S102、以左摄像机和右摄像机各自图像主点为中心，分别截取图像I_L中坐标为的矩形区域和图像I_R中坐标为的矩形区域(y_up和y_down为图像截取的上侧阈值和下侧阈值，w为图像I_L和I_R图像的宽度)可得截取后的RGB图像I_L,C，I_R,C；

上述的摄像头参数是通过单目标定获得的，如张正友标定法，

S103、对图像I_L,C，I_R,C分别灰度化，获得灰度图像I_L,G和I_R,G，再分别对I_L,G和I_R,G进行Canny算子边缘检测，获得二值化图像I_L,B和I_R,B；

S104、采用累计概率霍夫变换算法对二值化图像I_L,B和I_R,B进行直线线段检测，并提取其中斜率为的线段(T_d为角度变化阈值)，每条线段表示为式中是线段L_a,i与直线y^I＝y_up+¹交点的x^I值，并根据既定规则对两幅图像中得到的线段进行筛选，进一步可获得左图像中的线段组L_L＝{L_L,1,L_L,2,…,L_L,nL}和右图像中的线段组L_R＝{L_R,1,L_R,2,…,L_R,nR}(nL为左图像中线段的数量，nR为右图像中线段的数量)；

S105、以左图像的线段组L_L中线段L_L,i为基准，计算L_L,i对应于右图像中线段组L_R匹配范围内所有线段的匹配度，可得到符合筛选阈值的疑似匹配线段对组{L_L,i,:}＝{(L_L,i,L_R,i1),(L_L,i,L_R,i2),…,(L_L,i,L_R,in)}；

S106、判断L_L中所有的线段是否完成疑似匹配线段对组的获取，若否，则转向步骤S105；

S107、得到线段组L_L中每一条线段对应线段组L_R中的疑似匹配线段对组集合CMP＝{{L_L,0,:},{L_L,1,:},…,{L_L,nL,:}}；

S108、采用全局最优匹配的原则从CMP中选择L_L和L_R中最优匹配线段对队列(其中ix和jx分别代表线段和线段在线段组L_L和L_R中的序号)，其对应的实际竖线组为L＝{L₁,L₂,…,L_n}；

S109、使用双目测距公式

计算出OMP Q对应的实际竖线组L在摄像头坐标系下的坐标其中，f'、为双目视觉摄像头参数，由单目标定获得，T为两个摄像头光轴间的间距，分别为相匹配的两条线段和中心坐标的x^I值，x^C和z^C为相匹配的两条线段对应的实际竖线在摄像头坐标系下的x^C-z^C坐标；

S110、根据公式(i和j表示线L_i和线L_j在L的序号)计算L中竖线L_i和竖线L_j的距离，并认为：

当d_i,j∈[W_D,min,W_D,max](W_D,min和W_D,max分别为家居环境门宽的最小值和最大值)时，线L_i和线L_j可能为门的两个外边框，记为疑似门

当d_i,j∈[W_F,min,W_F,max](W_F,min和W_F,max分别为家居环境门框宽的最小值和最大值)时，线L_i和线L_j可能为门框的两侧边线，记为疑似门框

计算L中所有线之间的距离，可得到疑似门组(ix和jx分别代表竖线L_ix,x和L_jx,x在竖线组L中的序号，D为疑似门标号，x代表疑似门在D_sus中的序号)，和疑似门框组(ix和jx分别代表竖线L_ix,x和L_jx,x在竖线组L中的序号，F为疑似门框标号，x代表疑似门框在F_sus中的序号)

S111、采用特征积分制原则从疑似门组D_sus中选择最优解，积分最大的疑似门为最优解，即实现对门的识别。

进一步的，在步骤S104中，所述获得左图像中的线段组L_L＝{L_L,1,L_L,2,…,L_L,nL}和右图像中的线段组L_R＝{L_R,1,L_R,2,…,L_R,nR}(nL为左图像中线段的数量，nR为右图像中线段的数量)，进一步包括：

S201、删去像素长度小于T_length的线段；

S202、当存在中心间距小于T_dis像素长度的两条线段时，删去长度较小的线段；

S203、对两图像中的线段按照x^I从小到大进行排序；

S204、最终获得左图像中的线段组L_L＝{L_L,1,L_L,2,…,L_L,nL}和右图像中的线段组L_R＝{L_R,1,L_R,2,…,L_R,nR}。

进一步的，在步骤S105中，所述以左图像的线段组L_L中线段L_L,i为基准，计算L_L,i对应于右图像中线段组L_R匹配范围内所有线段的匹配度，可得到符合筛选阈值的疑似匹配线段对组{L_L,i,:}＝{(L_L,i,L_R,i1),(L_L,i,L_R,i2),…,(L_L,i,L_R,in)}，进一步包括：

以左图像的线段组L_L＝{L_L,1,L_L,2,…,L_L,nL}中的线段L_L,i为待匹配项为例，设其坐标为匹配过程如下：

S301、根据双目测距公式结合实际测量深度范围可得视差范围[d_min,d_max]，即线段组L_R中与L_L,i相匹配线段L_R,j的x^I取值范围为L_R中所有满足要求的线段记为线段L_L,i的疑似匹配线段组；

S302、计算匹配线与待匹配线的匹配区域，默认为宽度为N_p，高为y_up+y_down+1的矩形区域，若该默认区域内存在其他线段，则匹配区域变为两条线段之间的矩形区域，若该默认区域超出图像边界，则匹配区域变为该线段和图像边界之间的矩形区域，且保证左右图像待匹配的两块矩形区域宽度相同；

S303、进一步计算获得线段L_L,i左右两侧与其疑似匹配线段组内一条线段L_R,_ix左右匹配区域并分别计算4块匹配区域的特征直方图(颜色空间直方图，如RGB、HSV、YUV等，纹理直方图，如LBP等，和其他特征直方图)；

S304、通过采用相似性度量(如欧式距离、马氏距离、巴氏距离等)分别计算线段L_L,i和L_R,ix左侧匹配区域和线段L_L,i和L_R,ix右侧匹配区域和的各种特征直方图的相似度为和(n为特征标号)，进而得到线段L_L,_i和L_R,ix左右两侧的相似度权重为：

式中和分别为线段L_L,i与待匹配线段L_R,ix左右两侧关于特征k的相似度，a_k为特征k匹配度的权重；

S305、记w_i,ix为和中较大者，若w_i,ix＞T_m，则认为待匹配线段L_R,ix为L_L,i的疑似匹配线段，构成疑似匹配线段对(L_L,i,L_R,ix)；

S306、判断是否有其它线段未完成匹配，若是，则转向步骤S302；

S307、进一步得到全部符合筛选阈值的疑似匹配线段对，构成疑似匹配线段对组{L_L,i,:}＝{(L_L,i,L_R,i1),(L_L,i,L_R,i2),…,(L_L,i,L_R,in)}，n为匹配线段对组中匹配对的个数。

进一步的，在步骤S108中，所述采用全局最优匹配的原则从CMP中选择L_L和L_R中最优匹配线段对队列(其中ix和jx分别代表线段和线段在线段组L_L和L_R中的序号)，其对应的实际竖线组为L＝{L₁,L₂,…,L_n}，进一步包括：

S401、从CMP提取出L_L和L_R中所有的匹配线段对队列MPQ＝{(L_L,i1,L_R,j1),(L_L,i2,L_R,j2),…,(L_L,in,L_R,jn)}(其中ix和jx分别代表线段L_L,ix和线段L_R,_jx在线段组L_L和L_R中的序号)，每个MPQ＝{(L_L,i1,L_R,j1),(L_L,i2,L_R,j2),…,(L_L,in,L_R,jn)}应符合以下规则：

1.匹配线段对队列中的每一个疑似匹配线段对中两条线段的标号均小于其直接后续疑似匹配线段对中两条线段的对应标号，即对于疑似匹配线段对和疑似匹配线段对需满足i_k＜i_k+1&&j_k＜j_k+1；

2.匹配线段对队列中的第一个疑似匹配线段对内两条线段的标号需满足当前匹配线段对队列中没有其他的疑似匹配线段对内两条线段的标号小于其对应编号，即对于疑似匹配线段对当前匹配线段对队列中不存在疑似匹配线段对满足i_k＜i₁||j_k＜j₁；

3.匹配线段对队列中的最后一个疑似匹配线段对内两条线段的标号需满足当前匹配线段对队列中没有其他的疑似匹配线段对内两条线段的标号大于其对应编号，即对于匹配线段对当前匹配线段对队列中不存在匹配线段对满足i_k＞i_n||j_k＞j_n；

S402、计算每一个匹配线段对队列的总权重：

式中i为匹配线段对队列的序号，n为当前匹配线段对队列匹配对的个数，rho_Li,Rj为由L_L中第Li-th条线段和L_R中第Rj-th条线段组成的疑似匹配线段对的匹配度；

S403、计算所有匹配线段对队列的总权重，权重值最大的匹配线段对队列即为全局最优解，记为

进一步的，在步骤S111中，所述特征积分制原则进一步包括：

以疑似门(ix和jx为线L_ix,i和L_jx,i在L中的序号，D为疑似门标号,i为疑似门在疑似门组D_sus中的序号)为例，初始积分为0，特征积分制计算方法包括：

S501、若疑似门左侧存在门边框，即疑似门的左边线L_ix右侧存在线L_m满足(L_ix,L_m)∈F_sus，则加分S_F,L；

S502、若疑似门右侧存在门边框，即疑似门的右边线L_jx左侧存在线L_n满足(L_n,L_jx)∈F_sus，则加分S_F,R；

对于线L_x在双目中左右摄像头中成像为匹配线段对(L_L,ix,L_R,jx)，对于线段L_L,ix和L_R,jx左右两侧的匹配度和有三种情况：

式中T_m为匹配度阈值；

S503、若疑似门的边线L_ix左右两侧匹配度和满足式(1)，则加分S_M,1；若满足式(2)则加分S_M,2；若满足式(3)，则减分S_M,3；

S504、若疑似门的边线L_jx左右两侧匹配度和满足式(1)，则加分S_M,1；若满足式(3)则加分S_M,2；若满足式(2)，则减分S_M,3；

S505、若疑似门存在左边框(L_ix,L_m)时，若边线L_m左右两侧匹配度和满足式(1)或式(2)，则加分S_M,2；若满足式(3)，则减分S_M,3；

S506、若疑似门存在右边框(L_n,L_jx)时，若边线L_n左右两侧匹配度和满足式(1)或式(3)，则加分S_M,2；若满足式(2)，则减分S_M,3；

S507、若在疑似门两侧线中间存在实际线的深度发生突变，即存在L_k于L_ix和L_jx之间，并满足(T_Z为距离突变阈值)，则加分S_z,1；

S508若在疑似门两侧线中间存在实际点的深度发生突变，即存在匹配角点P_m于L_ix和L_jx之间，并满足则加分S_z,1。

进一步的，在步骤S508中，所述匹配角点进一步包括：

所述匹配角点指由ORB角点检测算法、SURF角点检测、FAST角点检测等算法对图像I_L,C和I_R,C检测出的角点，并采用BF匹配算法或FLANN匹配算法得到的匹配角点GM＝{(P_L,i1,P_R,j1),(P_L,i2,P_R,j2),…,(P_L,in,P_R,jn)}，并由双目测距公式获得其在摄像头坐标系下的坐标

进一步的，在步骤S111中，所述采用特征积分制原则从疑似门组D_sus中选择最优解，积分最大的疑似门为最优解，进一步包括：

计算疑似门组D_sus中所有疑似门的积分S_sus＝{S₁,S₂,…,S_n}，取积分最大且满足S_max≥T_s(T_s是积分阈值)的疑似门为最优解；若存在最大积分相同的疑似门，则选择疑似门中心点到摄像头坐标原点距离最短的疑似门为最优解。

本发明技术方案采用双目视觉摄像机，通过高效的算法有效实现家居环境下的门定位，而且不需要在门上增加多余的标志物，只需要门的两侧边框同时出现在双目视野中便可实现对门的定位，对处于关闭、半开、全开等各种状态的门都可以有效识别，具有成本低、使用方便灵活、应用场景广泛的优点。

本发明的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过在所写的说明书、权利要求书、以及附图中所特别指出的结构来实现和获得。

下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。

附图说明

附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例一起用于解释本发明，并不构成对本发明的限制。在附图中：

图1为本发明实施例一中基于双目视觉目标检测的家居环境下门定位方法的流程图；

图2为本发明实施例一中对左、右摄像机进行图像截取所使用的像素坐标系示意图；

图3为本发明实施例一中基于双目视觉目标检测的家居环境下门定位方法的流程示意图；

图4为本发明实施例一中步骤S109中摄像头坐标系示意图；

图5为本发明实施例一中步骤S501、S502中门的宽度信息积分示意图；

图6为本发明实施例一中对左右二路图像中判断可能为门边框的线段的方法流程图；

图7为本发明实施例一中寻找左右匹配线段的方法流程图；

图8为本发明实施例一中根据全局匹配原则获得匹配线段对组的方法流程图；

图9为本发明实施例一中根据特征积分制原则对疑似门边框线段进行加分的方法流程图。

具体实施方式

以下结合附图对本发明的优选实施例进行说明，应当理解，此处所描述的优选实施例仅用于说明和解释本发明，并不用于限定本发明。

如图1至图5所示，该流程包括以下步骤：

S101、将双目视觉摄像机(左摄像机和右摄像机)置于距离地面一定高度的支架上，水平放置，接收来自两路双目视觉摄像机的图像I_L、I_R；

S102、以左摄像机和右摄像机各自图像主点为中心，分别截取图像I_L中坐标为的矩形区域和图像I_R中坐标为的矩形区域(y_up和y_down为图像截取的上侧阈值和下侧阈值，w为图像I_L和I_R图像的宽度)可得截取后的RGB图像I_L,C，I_R,C；

上述的摄像头参数是通过单目标定获得的，如张正友标定法。

S103、对图像I_L,C，I_R,C分别灰度化，获得灰度图像I_L,G和I_R,G。再分别对I_L,G和I_R,G进行Canny算子边缘检测，获得二值化图像I_L,B和I_R,B；

S104、采用累计概率霍夫变换算法对二值化图像I_L,B和I_R,B进行直线线段检测，并提取其中斜率为的线段(T_d为角度变化阈值)。每条线段表示为式中是线段L_a,i与直线y^I＝y_up+1交点的x^I值。并根据既定规则对两幅图像中得到的线段进行筛选。进一步可获得左图像中的线段组L_L＝{L_L,1,L_L,2,…,L_L,nL}和右图像中的线段组L_R＝{L_R,1,L_R,2,…,L_R,nR}(nL为左图像中线段的数量，nR为右图像中线段的数量)；

S105、以左图像的线段组L_L中线段L_L,i为基准，计算L_L,i对应于右图像中线段组L_R匹配范围内所有线段的匹配度，可得到符合筛选阈值的疑似匹配线段对组{L_L,i,:}＝{(L_L,i,L_R,i1),(L_L,i,L_R,i2),…,(L_L,i,L_R,in)}；

S106、判断L_L中所有的线段是否完成疑似匹配线段对组的获取，若否，则转向步骤S105；

S107、得到线段组L_L中每一条线段对应线段组L_R中的疑似匹配线段对组集合CMP＝{{L_L,0,:},{L_L,1,:},…,{L_L,nL,:}}；

S108、采用全局最优匹配的原则从CMP中选择L_L和L_R中最优匹配线段对队列(其中ix和jx分别代表线段和线段在线段组L_L和L_R中的序号)，其对应的实际竖线组为L＝{L₁,L₂,…,L_n}；

S109、使用双目测距公式

计算出OMPQ对应的实际竖线组L在摄像头坐标系下的坐标其中，f'、为双目视觉摄像头参数，由单目标定获得，T为两个摄像头光轴间的间距，分别为相匹配的两条线段和中心坐标的x^I值，x^C和z^C为相匹配的两条线段对应的实际竖线在摄像头坐标系下的x^C-z^C坐标；

S110、根据公式(i和j表示线L_i和线L_j在L的序号)计算L中竖线L_i和竖线L_j的距离，并认为：

当d_i,j∈[W_D,min,W_D,max](W_D,min和W_D,max分别为家居环境门宽的最小值和最大值)时，线L_i和线L_j可能为门的两个外边框，记为疑似门

当d_i,j∈[W_F,min,W_F,max](W_F,min和W_F,max分别为家居环境门框宽的最小值和最大值)时，线L_i和线L_j可能为门框的两侧边线，记为疑似门框

计算L中所有线之间的距离，可得到疑似门组(ix和jx分别代表竖线L_ix,x和L_jx,x在竖线组L中的序号，D为疑似门标号，x代表疑似门在D_sus中的序号)，和疑似门框组(ix和jx分别代表竖线L_ix,x和L_jx,x在竖线组L中的序号，F为疑似门框标号，x代表疑似门框在F_sus中的序号)

S111、采用特征积分制原则从疑似门组D_sus中选择最优解，积分最大的疑似门为最优解，即实现对门的识别。

图6为本发明实施例一中对左右二路图像中判断可能为门边框的线段的方法流程图，即在步骤S104中，获得左、右图像中的线段组L_L和L_R的方法步骤。如图6所示，该流程包括以下步骤：

S201、删去像素长度小于T_length的线段；

S202、当存在中心间距小于T_dis像素长度的两条线段时，删去长度较小的线段；

S203、对两图像中的线段按照x^I从小到大进行排序；

S204、最终获得左图像中的线段组L_L＝{L_L,1,L_L,2,…,L_L,nL}和右图像中的线段组L_R＝{L_R,1,L_R,2,…,L_R,nR}。

图7为本发明实施例一中寻找左右匹配线段的方法流程图，即在步骤S105中，以左图像的线段组L_L中线段L_L,i为基准，计算L_L,i对应于右图像中线段组L_R匹配范围内所有线段的匹配度，得到符合筛选阈值的疑似匹配线段对组{L_L,i,:}＝{(L_L,i,L_R,i1),(L_L,i,L_R,i2),…,(L_L,i,L_R,in)}的流程。如图7所示，该流程包括以下步骤：

S301、根据双目测距公式结合实际测量深度范围可得视差范围[d_min,d_max]，即线段组L_R中与L_L,i相匹配线段L_R,j的xI取值范围为L_R中所有满足要求的线段记为线段L_L,i的疑似匹配线段组；

S302、计算匹配线与待匹配线的匹配区域，默认为宽度为N_p，高为y_up+y_down+1的矩形区域，若该默认区域内存在其他线段，则匹配区域变为两条线段之间的矩形区域，若该默认区域超出图像边界，则匹配区域变为该线段和图像边界之间的矩形区域，且保证左右图像待匹配的两块矩形区域宽度相同；

S303、进一步计算获得线段L_L,i左右两侧与其疑似匹配线段组内一条线段L_R,ix左右匹配区域并分别计算4块匹配区域的特征直方图(颜色空间直方图，如RGB、HSV、YUV等，纹理直方图，如LBP等，和其他特征直方图)；

S304、通过采用相似性度量(如欧式距离、马氏距离、巴氏距离等)分别计算线段L_L,i和L_R,ix左侧匹配区域和线段L_L,i和L_R,ix右侧匹配区域和的各种特征直方图的相似度为和(n为特征标号)，进而得到线段L_L,_i和L_R,ix左右两侧的相似度权重为：

式中和分别为线段L_L,i与待匹配线段L_R,ix左右两侧关于特征k的相似度，a_k为特征k匹配度的权重；

S305、记w_i,ix为和中较大者。若w_i,ix＞T_m，则认为待匹配线段L_R,ix为L_L,i的疑似匹配线段，构成疑似匹配线段对(L_L,i,L_R,ix)；

S306、判断是否有其它线段未完成匹配，若是，则转向步骤S302；

S307、进一步得到全部符合筛选阈值的疑似匹配线段对，构成疑似匹配线段对组{L_L,i,:}＝{(L_L,i,L_R,i1),(L_L,i,L_R,i2),…,(L_L,i,L_R,in)}，n为匹配线段对组中匹配对的个数。

图8为本发明实施例一中根据全局匹配原则获得匹配线段对组的方法流程图，即步骤S108中，采用全局最优匹配的原则从CMP中选择L_L和L_R中最优匹配线段对队列(其中ix和jx分别代表线段和线段在线段组L_L和L_R中的序号，其对应的实际竖线组为L＝{L₁,L₂,…,L_n})的方法。如图8所示，该流程包括以下步骤：

S401、从CMP提取出L_L和L_R中所有的匹配线段对队列MPQ＝{(L_L,i1,L_R,j1),(L_L,i2,L_R,j2),…,(L_L,in,L_R,jn)}(其中ix和jx分别代表线段L_L,ix和线段在线段组L_L和L_R中的序号)，每个MPQ＝{(L_L,i1,L_R,j1),(L_L,i2,L_R,j2),…,(L_L,in,L_R,jn)}应符合以下规则：

1.匹配线段对队列中的每一个疑似匹配线段对中两条线段的标号均小于其直接后续疑似匹配线段对中两条线段的对应标号，即对于疑似匹配线段对和疑似匹配线段对需满足i_k＜i_k+1&&j_k＜j_k+1；

2.匹配线段对队列中的第一个疑似匹配线段对内两条线段的标号需满足当前匹配线段对队列中没有其他的疑似匹配线段对内两条线段的标号小于其对应编号，即对于疑似匹配线段对当前匹配线段对队列中不存在疑似匹配线段对满足i_k＜i₁||j_k＜j₁；

3.匹配线段对队列中的最后一个疑似匹配线段对内两条线段的标号需满足当前匹配线段对队列中没有其他的疑似匹配线段对内两条线段的标号大于其对应编号，即对于匹配线段对当前匹配线段对队列中不存在匹配线段对满足i_k＞i_n||j_k＞j_n；

S402、计算每一个匹配线段对队列的总权重：

式中i为匹配线段对队列的序号，n为当前匹配线段对队列匹配对的个数，rho_Li,Rj为由L_L中第Li-th条线段和L_R中第Rj-th条线段组成的疑似匹配线段对的匹配度；

S403、计算所有匹配线段对队列的总权重，权重值最大的匹配线段对队列即为全局最优解，记为

图9为本发明实施例一中根据特征积分制原则对疑似门边框线段进行加分的方法流程图，即步骤S111中，特征积分制计算方法。如图9所示，该流程包括以下步骤：

S501、若疑似门左侧存在门边框，即疑似门的左边线L_ix右侧存在线L_m满足(L_ix,L_m)∈F_sus，则加分S_F,L；

S502、若疑似门右侧存在门边框，即疑似门的右边线L_jx左侧存在线L_n满足(L_n,L_jx)∈F_sus，则加分S_F,R；

对于线L_x在双目中左右摄像头中成像为匹配线段对(L_L,ix,L_R,jx)，对于线段L_L,ix和L_R,jx左右两侧的匹配度和有三种情况：

式中T_m为匹配度阈值；

S503、若疑似门的边线L_ix左右两侧匹配度和满足式(1)，则加分S_M,1；若满足式(2)则加分S_M,2；若满足式(3)，则减分S_M,3；

S504、若疑似门的边线L_jx左右两侧匹配度和满足式(1)，则加分S_M,1；若满足式(3)则加分S_M,2；若满足式(2)，则减分S_M,3；

S505、若疑似门存在左边框(L_ix,L_m)时，若边线L_m左右两侧匹配度和满足式(1)或式(2)，则加分S_M,2；若满足式(3)，则减分S_M,3；

S506、若疑似门存在右边框(L_n,L_jx)时，若边线L_n左右两侧匹配度和满足式(1)或式(3)，则加分S_M,2；若满足式(2)，则减分S_M,3；

S507、若在疑似门两侧线中间存在实际线的深度发生突变，即存在L_k于L_ix和L_jx之间，并满足(T_Z为距离突变阈值)，则加分S_z,1；

S508、若在疑似门两侧线中间存在实际点的深度发生突变，即存在匹配角点P_m于L_ix和L_jx之间，并满足则加分S_z,1。

匹配角点指由ORB角点检测算法、SURF角点检测、FAST角点检测等算法对图像I_L,C和I_R,C检测出的角点，并采用BF匹配算法或FLANN匹配算法得到的匹配角点GM＝{(P_L,i1,P_R,j1),(P_L,i2,P_R,j2),…,(P_L,in,P_R,jn)}，并由双目测距公式获得其在摄像头坐标系下的坐标

上述实施例中的技术方案由于采用双目视觉摄像机，通过高效的算法有效实现家居环境下的门定位，而且不需要在门上增加多余的标志物，只需要门的两侧边框同时出现在双目视野中便可实现对门的定位，对处于关闭、半开、全开等各种状态的门都可以有效识别，具有成本低、使用方便灵活、应用场景广泛的优点。

本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器和光学存储器等)上实施的计算机程序产品的形式。

本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。