三维位置估计装置以及程序的制作方法

本发明涉及三维位置估计装置以及程序。

背景技术：

以往，有利用安装于车辆的照相机拍摄周边环境，并根据拍摄到的图像制成环境地图的技术(非专利文献1)。

在非专利文献1中，从图像检测特征点，使用该特征点的局部特征量，对在多个地点拍摄到的图像间的特征点建立对应关系，根据建立了对应关系的特征点的图像上的位置计算照相机的位置/姿势和特征点的三维位置，生成由多个特征点的规定三维位置的特征点节点、多个拍摄地点的规定照相机的位置/姿势的照相机节点、规定各拍摄地点的图像上的特征点位置的特征点边缘、以及规定相邻的照相机节点间的车辆的位置/姿势的变化量的拍摄边缘构成的图形结构数据，并优化图形结构数据以使根据特征点边缘和拍摄边缘计算出的误差函数之和最小，由此制成环境地图。

非专利文献1：jingwang，zongyingshi，yishengzhong，"visualslamincorporatingwheelodometerforindoorrobots"，201736thchinesecontrolconference(ccc)，2017.

但是，在上述非专利文献1中，未对拍摄边缘的误差函数的权重(非专利文献中的imformationmatrixσb)的设定方法进行描述。

因此，在车辆转弯时那样的不能够根据里程以较高的精度获取车辆的位置/姿势的变化量的情况下，优化图形结构数据，以使得拍摄边缘的误差函数相对于车辆转弯时的精度较低的车辆的位置变化量的测量值变小，所以有根据车载照相机的图像制成的环境地图的失真增大这样的问题。

技术实现要素：

本发明是为了解决上述问题点而完成的，目的在于提供即使在不能够以较高的精度获取车辆的位置/姿势的变化量的情况下，也能够精度良好地估计特征点的三维位置的三维位置估计装置以及程序。

为了实现上述目的，本发明的三维位置估计装置构成为包含：特征点检测部，从由安装于移动体的拍摄装置拍摄到的多个图像中的每个图像检测多个特征点；位置姿势变化量计算部，基于与上述移动体相关的传感器信息，计算上述移动体的位置以及姿势的变化量；特征点关联部，使用表示由上述特征点检测部检测出的特征点的特征的特征量，对上述多个图像间的特征点建立对应关系；三维位置获取部，获取由上述特征点关联部建立了对应关系的特征点中的每个特征点的三维位置、和上述多个图像中的每个图像的上述拍摄装置的位置以及姿势；图形结构数据生成部，生成由各上述特征点的特征点节点、上述多个图像中的每个图像的拍摄节点、连接各上述特征点节点与各上述拍摄节点之间的特征点边缘、以及连接各相邻的上述拍摄节点间的拍摄边缘构成的图形结构数据，上述特征点节点规定上述特征点的三维位置，上述拍摄节点规定拍摄上述图像的上述拍摄装置的位置以及姿势，上述特征点边缘规定上述拍摄节点的上述图像上的上述特征点节点的特征点的位置，上述拍摄边缘规定上述拍摄节点的图像间的上述移动体的位置以及姿势的变化量；误差函数设定部，对由上述图形结构数据生成部生成的上述图形结构数据所包含的各上述特征点边缘，设定表示使用上述拍摄节点所规定的上述拍摄装置的位置以及姿势将被上述特征点边缘连接的上述特征点节点的特征点投影到上述拍摄节点的图像上时的投影误差的第一误差函数，并且对上述图形结构数据所包含的各上述拍摄边缘，设定表示上述拍摄边缘所规定的上述位置以及姿势的变化量与位置姿势变化量计算部的计算结果的误差的第二误差函数；以及图形结构数据优化部，优化上述图形结构数据的各上述特征点节点所规定的上述特征点的三维位置、以及各上述拍摄节点所规定的上述拍摄装置的位置以及姿势，以使各上述特征点边缘的上述第一误差函数与各上述拍摄边缘的上述第二误差函数之和最小，上述误差函数设定部对各上述拍摄边缘，设定表示与上述拍摄边缘对应的通过上述位置姿势变化量计算部得到的上述移动体的姿势的变化量越小越包含较大的权重的上述第二误差函数。

另外，本发明的程序是用于使计算机作为以下功能部发挥作用的程序，即：特征点检测部，从由安装于移动体的拍摄装置拍摄到的多个图像中的每个图像检测多个特征点；位置姿势变化量计算部，基于与上述移动体相关的传感器信息，计算上述移动体的位置以及姿势的变化量；特征点关联部，使用表示由上述特征点检测部检测出的特征点的特征的特征量，对上述多个图像间的特征点建立对应关系；三维位置获取部，获取由上述特征点关联部建立了对应关系的特征点中的每个特征点的三维位置、和上述多个图像中的每个图像的上述拍摄装置的位置以及姿势；图形结构数据生成部，生成由各上述特征点的特征点节点、上述多个图像中的每个图像的拍摄节点、连接各上述特征点节点与各上述拍摄节点之间的特征点边缘、以及连接各相邻的上述拍摄节点间的拍摄边缘构成的图形结构数据，上述特征点节点规定上述特征点的三维位置，上述拍摄节点规定拍摄上述图像的上述拍摄装置的位置以及姿势，上述特征点边缘规定上述拍摄节点的上述图像上的上述特征点节点的特征点的位置，上述拍摄边缘规定上述拍摄节点的图像间的上述移动体的位置以及姿势的变化量；误差函数设定部，对由上述图形结构数据生成部生成的上述图形结构数据所包含的各上述特征点边缘，设定表示使用上述拍摄节点所规定的上述拍摄装置的位置以及姿势将被上述特征点边缘连接的上述特征点节点的特征点投影到上述拍摄节点的图像上时的投影误差的第一误差函数，并且对上述图形结构数据所包含的各上述拍摄边缘，设定表示上述拍摄边缘所规定的上述位置以及姿势的变化量与位置姿势变化量计算部的计算结果的误差的第二误差函数；以及图形结构数据优化部，优化上述图形结构数据的各上述特征点节点所规定的上述特征点的三维位置、以及各上述拍摄节点所规定的上述拍摄装置的位置以及姿势，以使各上述特征点边缘的上述第一误差函数与各上述拍摄边缘的上述第二误差函数之和最小，上述误差函数设定部对各上述拍摄边缘，设定表示与上述拍摄边缘对应的通过上述位置姿势变化量计算部得到的上述移动体的姿势的变化量越小越包含较大的权重的上述第二误差函数。

根据本发明的三维位置估计装置以及程序，特征点检测部从由安装于移动体的拍摄装置拍摄到的多个图像中的每个图像检测多个特征点，位置姿势变化量计算部基于与该移动体相关的传感器信息，计算该移动体的位置以及姿势的变化量，特征点关联部使用表示由该特征点检测部检测出的特征点的特征的特征量，对该多个图像间的特征点建立对应关系，三维位置获取部获取由该特征点关联部建立了对应关系的各特征点的三维位置、和该多个图像中的每个图像的该拍摄装置的位置以及姿势，图形结构数据生成部生成由各该特征点的规定该特征点的三维位置的特征点节点、该多个图像中的每个图像的规定拍摄该图像的该拍摄装置的位置以及姿势的拍摄节点、连接各该特征点节点与各该拍摄节点之间的、规定该拍摄节点的该图像上的该特征点节点的特征点的位置的特征点边缘、以及连接各相邻的该拍摄节点间的、规定该拍摄节点的图像间的该移动体的位置以及姿势的变化量的拍摄边缘构成的图形结构数据。

而且，误差函数设定部对由图形结构数据生成部生成的该图形结构数据所包含的各该特征点边缘，设定表示使用该拍摄节点所规定的该拍摄装置的位置以及姿势将被该特征点边缘连接的该特征点节点的特征点投影到该拍摄节点的图像上时的投影误差的第一误差函数，并且对该图形结构数据所包含的各该拍摄边缘，设定表示该拍摄边缘规定的该位置以及姿势的变化量与位置姿势变化量计算部的计算结果的误差的第二误差函数，图形结构数据优化部优化该图形结构数据的各该特征点节点所规定的该特征点的三维位置、以及各该拍摄节点所规定的该拍摄装置的位置以及姿势，以使各该特征点边缘的该第一误差函数与各该拍摄边缘的该第二误差函数之和最小，该误差函数设定部对各该拍摄边缘，设定表示与该拍摄边缘对应的通过该位置姿势变化量计算部得到的该移动体的姿势的变化量越小越包含较大的权重的该第二误差函数。

这样，生成由规定特征点的三维位置的特征点节点、规定拍摄装置的位置以及姿势的拍摄节点、连接各特征点节点与各拍摄节点之间的、规定拍摄节点的图像上的特征点节点的特征点的位置的特征点边缘、以及连接各相邻的拍摄节点间的、规定拍摄节点的图像间的移动体的位置以及姿势的变化量的拍摄边缘构成的图形结构数据，设定表示使用拍摄节点所规定的拍摄装置的位置以及姿势将被特征点边缘连接的特征点节点的特征点投影到拍摄节点的图像上时的投影误差的第一误差函数，并且将表示拍摄边缘规定的位置以及姿势的变化量的误差的第二误差函数设定为表示与该拍摄边缘对应的该移动体的姿势的变化量越小越包含较大的权重，优化图形结构数据的特征点的三维位置、以及拍摄装置的位置以及姿势以使第一误差函数与第二误差函数之和最小，从而即使在不能够以较高的精度获取车辆的位置/姿势的变化量的情况下，也能够精度良好地估计特征点的三维位置。

另外，本发明的三维位置估计装置的上述位置姿势变化量计算部能够基于由安装于上述移动体的转向操纵角传感器检测出的转向操纵角，计算上述移动体的横摆角变化量，上述误差函数设定部设定基于上述位置姿势变化量计算部的计算结果得到的、表示与上述拍摄边缘对应的上述移动体的横摆角变化量越小越包含较大的权重的上述第二误差函数。

如以上所说明的那样，根据本发明的三维位置估计装置以及程序，生成由规定特征点的三维位置的特征点节点、规定拍摄装置的位置以及姿势的拍摄节点、连接各特征点节点与各拍摄节点之间的规定拍摄节点的图像上的特征点节点的特征点的位置的特征点边缘、以及连接各相邻的拍摄节点间的规定拍摄节点的图像间的移动体的位置以及姿势的变化量的拍摄边缘构成的图形结构数据，设定表示使用拍摄节点所规定的拍摄装置的位置以及姿势将被特征点边缘连接的特征点节点的特征点投影到拍摄节点的图像上时的投影误差的第一误差函数，并且将表示拍摄边缘所规定的位置以及姿势的变化量的误差的第二误差函数设定为表示与该拍摄边缘对应的该移动体的姿势的变化量越小越包含较大的权重，并优化图形结构数据的特征点的三维位置、以及拍摄装置的位置以及姿势，以使第一误差函数与第二误差函数之和最小，从而得到即使在不能够以较高的精度获取车辆的位置/姿势的变化量的情况下，也能够精度良好地估计特征点的三维位置这样的效果。

附图说明

图1是表示本发明的实施方式中的概略结构的框图。

图2是表示车辆的位置/横摆角的变化量的图。

图3是表示特征点的三维位置的计算的图。

图4是表示照相机位置/姿势的变化量的计算的图。

图5是表示图形结构数据的构成的图。

图6是表示本发明的实施方式中的环境地图制成处理例程的内容的流程图。

图7是表示本发明的实施方式中的三维位置计算处理例程的内容的流程图。

附图标记说明

10…地图制成系统，100…拍摄装置，200…车轮速度传感器，210…转向操纵角传感器，300…地图制成装置，310…传感器信息输入部，320…里程计算部，330…图像输入部，340…关键帧选择部，350…特征点提取部，360…特征量计算部，370…特征点关联部，380…三维位置计算部，390…图形结构数据生成部，400…误差函数设定部，410…图形结构数据优化部，420…环境地图制成部，430…输出部。

具体实施方式

以下，参照附图对本发明的实施方式进行详细说明。

＜地图制成系统的构成＞

如图1所示，本发明的实施方式所涉及的地图制成系统10构成为具备拍摄装置100、车轮速度传感器200、转向操纵角传感器210、以及地图制成装置300。

拍摄装置100安装于移动体并拍摄周边环境。在本实施方式中，以移动体是车辆的情况为例进行说明。

具体而言，拍摄装置100是设置于车辆的上部等的单眼照相机，拍摄车辆的前方、后方等。拍摄装置100例如设置在车宽方向的大致中央部附近，并且配置为拍摄装置100的光轴与水平方向相比稍微朝向下侧。

另外，虽然在本实施方式中，作为拍摄装置100例示单眼照相机进行说明，但是并不限定于此，也可以是其它方式的照相机，例如立体照相机等。

而且，拍摄装置100将拍摄到的图像交给地图制成装置300。

车轮速度传感器200检测车辆的四个车轮的车轮速度，并将检测出的车轮速度交给地图制成装置300。

转向操纵角传感器210检测车辆的转向操纵角，并将检测到的车辆的转向操纵角交给地图制成装置300。

地图制成装置300具备cpu、ram、以及存储了用于执行后述的姿势角修正误差估计处理例程以及自位置估计处理例程的程序的rom，在功能上如以下所示那样构成。

地图制成装置300构成为具备传感器信息输入部310、里程计算部320、图像输入部330、关键帧选择部340、特征点检测部350、特征量计算部360、特征点关联部370、三维位置计算部380、图形结构数据生成部390、误差函数设定部400、图形结构数据优化部410、环境地图制成部420、以及输出部430。

传感器信息输入部310受理由车轮速度传感器200检测出的车轮速度、以及由转向操纵角传感器210检测到的转向操纵角的输入。

然后，传感器信息输入部310将受理的车轮速度以及转向操纵角交给里程计算部320。

里程计算部320基于与车辆相关的传感器信息，计算车辆的位置变化量(δxv，δyv)以及横摆角变化量δθv。

具体而言，里程计算部320首先基于车轮速度计算车辆的行驶距离，并且基于转向操纵角计算车辆的转弯半径。

接下来，如图2所示，里程计算部320根据车辆的行驶距离以及转弯半径，计算车辆坐标系上的车辆的位置变化量(δxv，δyv)和横摆角变化量δθv。

然后，里程计算部320将计算出的位置变化量(δxv，δyv)和横摆角变化量δθv交给三维位置计算部380、以及误差函数设定部400。另外，里程计算部320将各时刻的车辆的行驶距离交给关键帧选择部340。

图像输入部330从拍摄装置100受理图像的输入。

然后，图像输入部330将受理的图像交给关键帧选择部340。

关键帧选择部340选择从拍摄装置100输入的图像中的多个为了制成环境地图所使用的关键帧。

具体而言，关键帧选择部340首先选择最初由拍摄装置100受理的图像作为第一个关键帧。

接下来，每当从里程计算部320获取的车辆的行驶距离超过规定值(例如0.1m)，关键帧选择部340就选择在超过了规定值的时刻由拍摄装置100受理的图像作为下一个关键帧。

然后，关键帧选择部340将所选择的多个关键帧交给特征点检测部350。

特征点检测部350从由安装于车辆的拍摄装置100拍摄到的多个关键帧中的每个关键帧检测多个特征点。

具体而言，特征点检测部350使用fast(参考文献1)等算法从各关键帧的图像金字塔检测多个特征点。

[参考文献1]raulmur-artal，j.m.m.montielandjuand.tardos，"orb-slam:aversatileandaccuratemonocularslamsystem."，ieeetransactionsonrobotics，vol.31，no.5，2015，pp.1147-1163.

然后，特征点检测部350对多个关键帧中的每个关键帧，将该关键帧的多个特征点交给特征量计算部360。

特征量计算部360对多个关键帧中的每个关键帧，计算该关键帧的多个特征点中的每个特征点的特征量。

具体而言，特征量计算部360根据多个关键帧中的每个关键帧的多个特征点，计算orb(参考文献1)等局部特征量。

然后，特征量计算部360对多个关键帧中的每个关键帧，将该关键帧的多个特征点、和该多个特征点中的每个特征点的特征量交给特征点关联部370。

特征点关联部370使用由特征量计算部360计算出的表示特征点的特征的特征量，对连续的关键帧间的特征点建立对应关系。

具体而言，特征点关联部370使用各特征点的特征量，对连续的关键帧间的特征点建立对应关系。

然后，特征点关联部370对多个关键帧中的每个关键帧，将建立了对应关系的特征点交给三维位置计算部380。

三维位置计算部380计算由特征点关联部370建立了对应关系的各特征点的三维位置、和多个图像中的每个图像的拍摄装置100的位置以及姿势。

具体而言，三维位置计算部380从第二个关键帧开始依次基于与前一个关键帧之间的特征点的对应关系，计算各特征点的三维位置。

三维位置计算部380在第二个关键帧的情况下，首先，在与第一个关键帧之间，使用八点算法和ransac(randomsampleconsensus：随机抽样一致性)，计算第一个关键帧的拍摄位置与第二个关键帧的拍摄位置之间的拍摄装置100的位置以及姿势的变化量(无维量)，并使用车辆的位置变化量(δxv，δyv)进行有维化(决定绝对的标度)。

接下来，如图3所示，三维位置计算部380根据建立了对应关系的特征点的关键帧上的位置和拍摄装置100的位置以及姿势的变化量，通过三角测量的原理计算该特征点的三维位置(世界坐标系)。

如图4所示，在第三个以后的关键帧的情况下，首先，三维位置计算部380提取在与前一个关键帧之间建立了对应关系的特征点中三维位置已知的特征点，并计算其再投影误差之和最小的拍摄装置100的位置以及姿势的变化量。

接下来，三维位置计算部380对在与前一个关键帧之间建立了对应关系的特征点中三维位置未知的特征点，与第二个关键帧相同地，根据关键帧上的特征点的位置和拍摄装置100的位置以及姿势的变化量通过三角测量的原理计算该特征点的三维位置。

然后，三维位置计算部380将计算得到的多个特征点中的每个特征点的三维位置、和多个关键帧中的每个关键帧的、该关键帧的前一个关键帧的拍摄位置与该关键帧的拍摄位置之间的拍摄装置100的位置以及姿势的变化量交给图形结构数据生成部390。

图形结构数据生成部390生成由各特征点的规定该特征点的三维位置的特征点节点、多个关键帧中的每个关键帧的规定拍摄该关键帧的拍摄装置100的位置以及姿势的拍摄节点、连接各特征点节点与各拍摄节点之间的规定该拍摄节点的关键帧上的该特征点节点的特征点的位置的特征点边缘、以及连接各相邻的拍摄节点间的规定该拍摄节点的关键帧间的车辆的位置以及姿势的变化量的拍摄边缘构成的图形结构数据。

具体而言，如图5所示，图形结构数据生成部390生成由规定特征点pi的三维位置的特征点节点、规定关键帧kfj的照相机位置/姿势的照相机节点、规定关键帧kfj上的特征点pi的位置的特征点边缘、以及规定相邻的关键帧间的车辆的位置变化量和横摆角变化量的拍摄边缘构成的图形结构数据。

具体而言，图形结构数据生成部390使用由三维位置计算部380得到的各特征点的三维位置生成图形结构数据。

然后，图形结构数据生成部390将生成的图形结构数据交给误差函数设定部400。

误差函数设定部400对由图形结构数据生成部390生成的图形结构数据所包含的各特征点边缘，设定表示使用拍摄节点所规定的拍摄装置100的位置以及姿势将被特征点边缘连接的特征点节点的特征点投影到拍摄节点的图像上时的投影误差的第一误差函数cpij，并且对图形结构数据所包含的各拍摄边缘，设定表示该拍摄边缘规定的位置以及姿势的变化量与里程计算部320的计算结果的误差的第二误差函数ckfj。

具体而言，误差函数设定部400如下述式(1)、(2)那样设定特征点边缘的第一误差函数cpij。

【式1】

cpij＝e^tpijwpijepij…(1)

epij是表示关键帧kfj上的特征点pi的(x，y)坐标的再投影误差的两行一列的向量，能够表示为

这里，(xpij，ypij)是特征点pi的实际的坐标，带“^”(帽子标记)的是投影到图像上时的计算值。

wpij是对再投影误差的权重，对从图像金字塔的分辨率较高的阶层检测到的特征点设定得较大。

另外，误差函数设定部400如下述式(3)、(4)那样设定拍摄边缘的第二误差函数ckfj。

【式2】

ckfj＝e^tkfjwkfjekfj…(3)

这里，ekfj是表示根据关键帧kfj-1与kfj之间的照相机位置/姿势的变化量换算出的车辆的位置变化量和横摆角变化量的相对于测量值的误差的三行一列的向量，能够表示为

这里，带“^”的是计算值。

另外，wxj是针对车辆的x方向的位置变化量的误差的权重，wyj是针对车辆的y方向的位置变化量的误差的权重，wθj是针对车辆的横摆角变化量的误差的权重。

这里，误差函数设定部400对各拍摄边缘，设定表示与拍摄边缘对应的通过里程计算部320得到的车辆的横摆角变化量δθv越小越包含较大的权重wxj、wyj、以及wθj的第二误差函数ckfj。

即，越为车辆的横摆角变化量δθv较小的拍摄边缘，误差函数设定部400越较大地设定权重wxj、wyj、以及wθj。

这里，也可以设定为wxj＝wyj。由此，能够节约存储器容量。

然后，误差函数设定部400将图形结构数据、第一误差函数cpij、以及第二误差函数ckfj交给图形结构数据优化部410。

图形结构数据优化部410优化图形结构数据的各特征点节点所规定的该特征点的三维位置、以及各拍摄节点所规定的拍摄装置100的位置以及姿势，以使各特征点边缘的第一误差函数cpij与各拍摄边缘的第二误差函数ckfj之和最小。

具体而言，图形结构数据优化部410与非专利文献1相同地，优化图形结构数据的各特征点节点所规定的该特征点的三维位置、以及各拍摄节点所规定的拍摄装置100的位置以及姿势，以使各特征点边缘的第一误差函数cpij与各拍摄边缘的第二误差函数ckfj之和最小。

这里，通过越为车辆的横摆角变化量δθv较小的拍摄边缘越较大地设定第二误差函数ckfj的权重wxj、wyj、以及wθj，而将图形结构数据优化为车辆的位置变化量(δxv，δyv)的获取精度比较高的拍摄边缘的误差函数变小。由此，能够在整体上提高构成图形结构数据的特征点三维位置和照相机位置/姿势的精度。

然后，图形结构数据优化部410将进行了优化的图形结构数据交给环境地图制成部420。

环境地图制成部420基于由图形结构数据优化部410进行了优化的图形结构数据，制成包含各特征点的三维位置信息而构成的环境地图。

具体而言，环境地图制成部420与非专利文献1相同地，基于由图形结构数据优化部410进行了优化的图形结构数据，制成包含各特征点的三维位置信息而构成的环境地图。这里，由于与非专利文献1相比特征点的三维位置的精度较高，所以能够制成失真较小的地图。

然后，环境地图制成部420将制成的环境地图交给输出部430。

输出部430输出由环境地图制成部420制成的环境地图。

＜地图制成装置300的动作＞

接下来，参照图6对本实施方式的地图制成装置300的地图制成处理例程进行说明。

首先，在步骤s100中，地图制成装置300设为i＝1。这里，i是对选择的关键帧的数目进行计数的计数器。

在步骤s110中，传感器信息输入部310受理由车轮速度传感器200检测到的车轮速度、以及由转向操纵角传感器210检测到的转向操纵角的输入。

在步骤s120中，里程计算部320基于在上述步骤s110得到的与车辆相关的传感器信息，计算车辆的位置变化量(δxv，δyv)以及横摆角变化量δθv。

在步骤s130中，图像输入部330从拍摄装置100受理图像的输入。

在步骤s140中，关键帧选择部340判定是否i＝1。

在i＝1的情况下(上述步骤s140的是)，进入步骤s160。

另一方面，在不为i＝1的情况下(上述步骤s140的否)，在步骤s150中，关键帧选择部340判定车辆的行驶距离是否为规定值以上。

在行驶距离为规定值以上的情况下(上述步骤s150的是)，进入步骤s160。

另一方面，在行驶距离不为规定值以上的情况下(上述步骤s150的否)，返回到步骤s110。

在步骤s160中，关键帧选择部340选择由上述步骤s130获取的图像作为第i个关键帧。

在步骤s170中，关键帧选择部340判定是否选择了全部的关键帧。

在未选择全部的关键帧的情况下(上述步骤s170的否)，在步骤s180中，关键帧选择部340对i加1，并返回到步骤s110。

另一方面，在选择全部的关键帧的情况下(上述步骤s170的是)，在步骤s190中，特征点检测部350从多个关键帧中的每个关键帧检测多个特征点。

在步骤s200中，特征量计算部360对多个关键帧中的每个关键帧，计算该关键帧的多个特征点中的每个特征点的特征量。

在步骤s210中，特征点关联部370使用通过上述步骤s200计算出的表示特征点的特征的特征量，对多个关键帧间的特征点建立对应关系。

在步骤s220中，三维位置计算部380计算通过上述步骤s210建立了对应关系的各特征点的三维位置、和多个关键帧中的每个关键帧的拍摄装置100的位置以及姿势。

在步骤s230中，图形结构数据生成部390生成由各特征点的规定该特征点的三维位置的特征点节点、多个关键帧中的每个关键帧的规定拍摄该关键帧的拍摄装置100的位置以及姿势的拍摄节点、连接各特征点节点与各拍摄节点之间的规定该拍摄节点的关键帧上的该特征点节点的特征点的位置的特征点边缘、以及连接各相邻的拍摄节点间的规定该拍摄节点的关键帧间的车辆的位置以及姿势的变化量的拍摄边缘构成的图形结构数据。

在步骤s240中，误差函数设定部400对通过上述步骤s230生成的图形结构数据所包含的各特征点边缘，设定表示使用拍摄节点所规定的拍摄装置100的位置以及姿势将被特征点边缘连接的特征点节点的特征点投影到拍摄节点的关键帧上时的投影误差的第一误差函数cpij，并且对图形结构数据所包含的各拍摄边缘，设定表示该拍摄边缘所规定的位置以及姿势的变化量与里程计算部320的计算结果的误差的第二误差函数ckfj。

在步骤s250中，图形结构数据优化部410优化图形结构数据的各特征点节点所规定的该特征点的三维位置、以及各拍摄节点所规定的拍摄装置100的位置以及姿势，以使各特征点边缘的第一误差函数cpij与各拍摄边缘的第二误差函数ckfj之和最小。

在步骤s260中，环境地图制成部420基于通过上述步骤s250进行了优化的图形结构数据，制成包含各特征点的三维位置信息而构成的环境地图。

在步骤s270中，输出部430输出通过上述步骤s260制成的环境地图。

另外，通过图7所示的三维位置计算处理例程实现上述步骤s220。

在步骤s221中，三维位置计算部380设为i＝2。这里，i是表示拍摄关键帧的编号的计数器。

在步骤s222中，三维位置计算部380选择第i个关键帧。

在步骤s223中，三维位置计算部380判定是否i＝2。

在i＝2的情况下(上述步骤s227的是)，在步骤s224中，三维位置计算部380在与第一个关键帧之间，使用八点算法和ransac(randomsampleconsensus：随机抽样一致性)，计算第一个关键帧的拍摄位置与第二个关键帧的拍摄位置之间的拍摄装置100的位置以及姿势的变化量(无维量)，使用车辆的位置变化量(δxv，δyv)将其有维化，并进入步骤s226。

另一方面，在不为i＝2的情况下(上述步骤s227的否)，在步骤s225中，三维位置计算部380提取在与第i－1个关键帧之间建立了对应关系的特征点中三维位置已知的特征点，计算其再投影误差之和最小的拍摄装置100的位置以及姿势的变化量，并进入步骤s226。

在步骤s226中，三维位置计算部380对在与第i－1个关键帧之间建立了对应关系的特征点中三维位置未知的特征点，通过三角测量的原理根据第i个关键帧上的特征点的位置和拍摄装置100的位置以及姿势的变化量计算该特征点的三维位置。

在步骤s227中，三维位置计算部380判定是否对于全部的关键帧进行了处理。

在未对全部的关键帧进行处理的情况下(上述步骤s227的否)，在步骤s228中，三维位置计算部380对i加1，并返回到步骤s222。

另一方面，在对全部的关键帧进行了处理的情况下(上述步骤s227的是)，返回。

如以上所说明的那样，根据本发明的实施方式所涉及的地图制成装置，生成由规定特征点的三维位置的特征点节点、规定拍摄装置的位置以及姿势的拍摄节点、连接各特征点节点与各拍摄节点之间的规定拍摄节点的图像上的特征点节点的特征点的位置的特征点边缘、以及连接各相邻的拍摄节点间的规定拍摄节点的图像间的移动体的位置以及姿势的变化量的拍摄边缘构成的图形结构数据，设定表示使用拍摄节点所规定的拍摄装置的位置以及姿势将被特征点边缘连接的特征点节点的特征点投影到拍摄节点的图像上时的投影误差的第一误差函数，并且将表示拍摄边缘所规定的位置以及姿势的变化量的误差的第二误差函数设定为表示与该拍摄边缘对应的该移动体的姿势的变化量越小越包含较大的权重，优化图形结构数据的特征点的三维位置、以及拍摄装置的位置以及姿势，以使第一误差函数与第二误差函数之和最小，从而即使在不能够以较高的精度获取车辆的位置/姿势的变化量的情况下，也能够精度良好地估计特征点的三维位置。

此外，本发明并不限定于上述的实施方式，在不脱离该发明的主旨的范围内能够进行各种变形、应用。

例如，虽然在上述实施方式中，以地图制成装置300安装于车辆的情况为例进行了说明，但地图制成装置300也可以构成为网络上的服务器。

该情况下，拍摄装置100、车轮速度传感器200、以及转向操纵角传感器210构成为通过无线通信等经由互联网向地图制成装置300分别发送图像、车轮速度、以及转向操纵角即可。

另外，虽然在本申请说明书中，作为预先安装有程序的实施方式进行了说明，但也能够将该程序储存于计算机能够读取的记录介质来提供。