本发明属于计算机视觉、三维重构技术领域，尤其涉及一种基于双目视觉与单目视觉的多相机车辆环境三维重构和运动估计方法。
背景技术：
三维重构技术包括基于主动视觉的三维重构技术和基于被动视觉的三维重构技术。其中基于被动视觉的三维重构技术又包括双目(立体)视觉法和单目视觉法。双目视觉三维重构是采用两台相机,从左、右两个视点拍摄同一场景,然后通过左右图像间的匹配点恢复出场景中目标物体的三维信息。立体视觉方法重构效果较好，适应性强,使用范围广。不足的是运算量偏大,并且在基线距离较大的情况下重构效果不好。单目视觉三维重构是仅使用一台相机来进行三维重构,这种方法简单方便、灵活可靠、使用范围广。将双目视觉三维重构与单目视觉三维重构方法融合，并且把双目视觉三维重构过程中获得的车辆位姿信息传输给单目视觉系统，从而提高单目视觉三维重构的精度，实现重构效果好，成本低。经过检索，和本发明最接近的对比文件如下：
1.urbans,wursthorns,leitloffj,etal.multicolbundleadjustment:agenericmethodforposeestimation,simultaneousself-calibrationandreconstructionforarbitrarymulti-camerasystems[j].internationaljournalofcomputervision,2017,121(2):234-252.
2.urbans,hinzs.multicol-slam-amodularreal-timemulti-cameraslamsystem[j].2016.
技术实现要素：
本发明旨在解决以上现有技术的问题。提出了一种提高单目视觉的重构精度、降低成本的多相机车辆环境三维重构和运动估计系统及方法。本发明的技术方案如下：
一种多相机车辆环境三维重构和运动估计系统，其特征在于，包括：双目视觉三维重构模块、单目视觉三维重构模块及整车环境三维重构模块；其中，
双目视觉三维重构模块：用于将位于车辆前端标定过的双目摄像头采集回来的图像进行特征点提取与匹配，从而得到稠密的三维点云，同时对双目摄像头拍摄的同一时刻的左、右帧图像进行特征点匹配以及同一台相机拍摄的前、后帧图像进行特征点匹配，获得车辆的运动估计，并传输给单目视觉三维重构模块；
单目视觉三维重构模块：用于对每台标定过的单目摄像头采集的图像进行特征点提取与匹配，并利用双目视觉三维重构系统得到的车辆运动估计，可以求得单目相机前后时刻的旋转平移参数，再对前后帧图像上的特征点进行匹配，获得精确的三维稠密点云；
整车环境三维重构模块：用于将所述双目视觉三维重构模块和单目视觉三维重构模块获得的三维稠密点云进行坐标转换拼接，并通过光束法平差方法对每个三维坐标点进行优化，从而获得优化后的整车环境的三维环境重构结果。
进一步的，所述双目摄像头有两个摄像头，用于拍摄车前的场景，单目摄像头有3个摄像头，分别用于拍摄行车右、后、左边的场景。
进一步的，所述双目摄像头和单目摄像头均为广角摄像头。
进一步的，所述车辆的速度在0.1s内呈线性变化。
进一步的，所述双目视觉三维重构模块将位于车辆前端标定过的双目摄像头采集回来的图像进行特征点提取算法具体包括步骤：尺度空间的极值检测；特征点定位；特征方向赋值；特征点描述。
进一步的，所述对双目相机拍摄的同一时刻的左、右帧图像进行特征点匹配以及同一台相机拍摄的前、后帧图像进行特征点匹配具体包括；左、右匹配利用极限约束将对应特征点的搜索范围缩小到一条直线上，前、后匹配则在竖直方向上增大搜索范围。
进一步的，根据匹配好的特征点及其3d坐标，利用重投影方法获得估计坐标值，再将估计的坐标值与实际的坐标值进行参数优化，获得前后两个时刻相机的运动，从而得到车辆当前的运动估计；利用匹配结果对双目相机拍摄的车辆前方的场景进行三维重构，获得以左相机光学中心为坐标原点的稠密的三维点云。
进一步的，所述单目视觉三维重构模块对于每台单目相机拍摄的场景，由于双目相机相对于车身的位姿为已知，并且根据每台相机之间的旋转平移参数，可以求得每台单目相机相对于车身的位姿，再利用获得的车辆当前的运动估计，得到每台单目相机的运动估计，再对前后帧图像进行特征点检测与匹配，从而获得精确的稠密三维点云。
一种基于所述系统的多相机车辆环境三维重构和运动估计方法，其包括以下步骤：
步骤1、对5个摄像头分别进行标定，获得每台相机的内参，以及各台相机之间的旋转平移参数，并对获取的图像进行畸变矫正；
步骤2、用特征点提取算法分别对双目摄像头拍摄的左、右图像进行特征点提取；
步骤3、对双目相机拍摄的同一时刻的左、右帧图像进行特征点匹配以及同一台相机拍摄的前、后帧图像进行特征点匹配；其中左、右匹配利用极限约束将对应特征点的搜索范围缩小到一条直线上，前、后匹配则在竖直方向上增大搜索范围，；
步骤4、根据步骤3中匹配好的特征点及其3d坐标，利用重投影方法获得估计坐标值，再将估计的坐标值与实际的坐标值进行参数优化，获得前后两个时刻相机的运动，从而得到车辆当前的运动估计；
步骤5、利用步骤三中的匹配结果对双目相机拍摄的车辆前方的场景进行三维重构，获得以左相机光学中心为坐标原点的稠密的三维点云；
步骤6、对于每台单目相机拍摄的场景，由于双目相机相对于车身的位姿为已知，并且每台相机之间的旋转平移参数可由步骤一标定中得到，因此，可以求得每台单目相机相对于车身的位姿，再利用步骤4中获得的车辆当前的运动估计，可以得到每台单目相机的运动估计，再对前后帧图像进行特征点检测与匹配，从而获得精确的稠密三维点云；
步骤7、根据已知的双目相机相对于车身的位姿，可以得到双目相机在车身坐标系o下的三维稠密点云；并且，根据步骤6中获得的每台单目相机相对于车身的位姿，获得单目相机在车身坐标系o下的三维稠密点云，所得即为整车环境的三维重构地图；
步骤8、利用光束法平差方法对每一个三维空间点进行优化，消除噪声。
进一步的，所述步骤7中，将双目相机相机1坐标系下的三维点云转到车身坐标系o下，可由如下公式计算：
(x1',y1',z1')＝(r1,t1)·(x1,y1,z1)，r1和t1分别表示相机1坐标系到车身坐标系的旋转参数矩阵和平移参数矩阵，(x1,y1,z1)为双目视觉系统重构出的一个三维空间点(以左相机c1为坐标原点)；
将相机3坐标系下的三维点云转到车身坐标系o下，可由如下公式计算：
(x3',y3',z3')＝(r3,t3)·(x3,y3,z3)
将相机4坐标系下的三维点云转到车身坐标系o下，可由如下公式计算：
(x'4,y4',z'4)＝(r4,t4)·(x4,y4,z4)
将相机5坐标系下的三维点云转到车身坐标系o下，可由如下公式计算：
(x5',y5',z5')＝(r5,t5)·(x5,y5,z5)
其中，(x1,y1,z1)为双目视觉系统重构出的一个三维空间点(以左相机c1为坐标原点)；(x3,y3,z3)为单目视觉系统中相机3(c3)重构出的一个三维空间点，其余点类似。
本发明的优点及有益效果如下：
1、本发明中的单目视觉三维重构模块将双目视觉三维重构模块中获得的车辆的运动估计(步骤四)运用于单目视觉的三维重构，获得更精确的三维稠密点云，提高重构精度。本发明避免了使用的雷达造成的高成本，并且在低成本的条件下，能够实现整车环境的三维重构。
附图说明
图1是本发明提供优选实施例方法的流程简图；
图2为本发明中车辆摄像头布置示意图；
图3为本发明中坐标系转换示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、详细地描述。所描述的实施例仅仅是本发明的一部分实施例。
本发明解决上述技术问题的技术方案是：
本发明的目的是提供一种基于双目视觉与单目视觉的多相机车辆环境三维重构和运动估计方法，通过5个摄像头(其中车前侧2个摄像头，车右侧1个摄像头，车后侧一个摄像头，车左侧一个摄像头)采集车辆周围图像信息进行处理，获取稠密三维点云，并记录了车辆的运动估计，从而实现整车环境的三维重构与车辆运动估计功能。为了达到上述目的，本发明采用如下技术方案：一种基于双目视觉与单目视觉的多相机车辆环境三维重构和运动估计方法，包括：
双目视觉三维重构：将位于车辆前端标定过的双目摄像头采集回来的图像进行特征点提取与匹配，从而得到稠密的三维点云，同时对双目相机拍摄的同一时刻的左、右帧图像进行特征点匹配以及同一台相机拍摄的前、后帧图像进行特征点匹配，获得车辆的运动运动估计。
单目视觉三维重构：对每台标定过的单目摄像头采集的图像进行特征点提取与匹配，并利用双目视觉三维重构系统得到的车辆运动估计，获得精确的三维稠密点云。
整车环境三维重构：将前面双目视觉三维重构模块和单目视觉三维重构模块获得的三维稠密点云进行坐标转换拼接，并通过光束法平差方法对每个三维坐标点进行优化，从而获得优化后的整车环境的三维环境重构结果。
可选的，所述一种基于双目视觉与单目视觉的多相机车辆环境三维重构和运动估计方法包括五个摄像头，其中车前侧2个摄像头拍摄车前的路面场景，车右侧1个摄像头拍摄车右侧的路面场景，车后侧一个摄像头拍摄车后侧的路面场景，车左侧一个摄像头拍摄车左侧的路面场景。
可选的，所述一种基于双目视觉与单目视觉的多相机车辆环境三维重构和运动估计方法所采用的摄像头为广角摄像头。
可选的，所述一种基于双目视觉与单目视觉的多相机车辆环境三维重构和运动估计方法包括一组双目摄像头，拍摄车前的路面场景。
如图1所示，本发明的技术方案是一种基于双目视觉与单目视觉的多相机车辆环境三维重构和运动估计方法，包括如下步骤：
步骤1、对5个摄像头分别进行标定，获得每台相机的内参，以及各台相机之间的旋转平移参数，并对获取的图像进行畸变矫正；
步骤2、用特征点提取算法分别对双目摄像头拍摄的左、右图像进行特征点提取；
步骤3、对双目相机拍摄的同一时刻的左、右帧图像进行特征点匹配以及同一台相机拍摄的前、后帧图像进行特征点匹配；其中左、右(立体)匹配利用极限约束将对应特征点的搜索范围缩小到一条直线上，前、后匹配则在竖直方向上增大搜索范围，；
步骤4、根据步骤3中匹配好的特征点及其3d坐标，利用重投影方法获得估计坐标值，再将估计的坐标值与实际的坐标值进行参数优化，获得前后两个时刻相机的运动，从而得到车辆当前的运动估计；
步骤5、利用步骤三中的匹配结果对双目相机拍摄的车辆前方的场景进行三维重构，获得以左相机光学中心为坐标原点的稠密的三维点云；
步骤6、对于每台单目相机拍摄的场景，由于双目相机相对于车身的位姿为已知，并且每台相机之间的旋转平移参数可由步骤一标定中得到，因此，可以求得每台单目相机相对于车身的位姿，再利用步骤4中获得的车辆当前的运动估计，可以得到每台单目相机的运动估计，再对前后帧图像进行特征点检测与匹配，从而获得精确的稠密三维点云；
步骤7、根据已知的双目相机相对于车身的位姿，可以得到双目相机在车身坐标系o下的三维稠密点云；并且，根据步骤六中获得的每台单目相机相对于车身的位姿，获得单目相机在车身坐标系o下的三维稠密点云，所得即为整车环境的三维重构地图；
将相机1坐标系下的三维点云转到车身坐标系o下，可由如下公式计算：
(x1',y1',z1')＝(r1,t1)·(x1,y1,z1)
将相机3坐标系下的三维点云转到车身坐标系o下，可由如下公式计算：
(x3',y3',z3')＝(r3,t3)·(x3,y3,z3)
将相机4坐标系下的三维点云转到车身坐标系o下，可由如下公式计算：
(x'4,y4',z'4)＝(r4,t4)·(x4,y4,z4)
将相机5坐标系下的三维点云转到车身坐标系o下，可由如下公式计算：
(x5',y5',z5')＝(r5,t5)·(x5,y5,z5)
其中，(x1,y1,z1)为双目视觉系统重构出的一个三维空间点(以左相机c1为坐标原点)；(x3,y3,z3)为单目视觉系统中相机3(c3)重构出的一个三维空间点，其余点类似。
步骤8、利用光束法平差方法对每一个三维空间点进行优化，消除噪声。
以上这些实施例应理解为仅用于说明本发明而不用于限制本发明的保护范围。在阅读了本发明的记载的内容之后，技术人员可以对本发明作各种改动或修改，这些等效变化和修饰同样落入本发明权利要求所限定的范围。