WO2022118513A1 - 位置姿勢算出装置、位置姿勢算出方法及び測量装置 - Google Patents

位置姿勢算出装置、位置姿勢算出方法及び測量装置 Download PDF

Info

Publication number
WO2022118513A1
WO2022118513A1 PCT/JP2021/032573 JP2021032573W WO2022118513A1 WO 2022118513 A1 WO2022118513 A1 WO 2022118513A1 JP 2021032573 W JP2021032573 W JP 2021032573W WO 2022118513 A1 WO2022118513 A1 WO 2022118513A1
Authority
WO
WIPO (PCT)
Prior art keywords
information
moving body
posture
image
acquisition unit
Prior art date
Application number
PCT/JP2021/032573
Other languages
English (en)
French (fr)
Inventor
遼雅 鈴木
義広 都丸
浩平 岡原
賢人 山▲崎▼
謙二 平
Original Assignee
三菱電機株式会社
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 三菱電機株式会社 filed Critical 三菱電機株式会社
Publication of WO2022118513A1 publication Critical patent/WO2022118513A1/ja

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
    • G01B11/00—Measuring arrangements characterised by the use of optical techniques
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C15/00—Surveying instruments or accessories not provided for in groups G01C1/00 - G01C13/00

Definitions

  • This disclosure relates to a position / attitude calculation device, a position / attitude calculation method, and a surveying device.
  • Patent Document 1 discloses a surveying device that measures a three-dimensional shape of a surveying object.
  • the surveying device includes an imager that captures an image of an object to be surveyed, a measuring instrument that measures the position of a moving body equipped with the imager, and a posture detector that detects the posture of the moving body.
  • the surveying device calculates the three-dimensional shape of the object to be surveyed by using the image captured by the imager, the position measured by the measuring device, and the posture detected by the posture detector.
  • the surveying device in order to realize highly accurate calculation of a three-dimensional shape, the surveying device is equipped with a posture detector capable of detecting the posture of a moving body with high accuracy. Must be done. However, a posture detector capable of detecting a posture with high accuracy is generally expensive.
  • the present disclosure has been made to solve the above-mentioned problems, and obtains a position / attitude calculation device and a position / attitude calculation method capable of calculating the posture of a moving body without mounting a posture detector. With the goal.
  • the position / orientation calculation device calculates the position of the moving body and the posture of the moving body provided with the imager that captures the object to be measured a plurality of times and generates a plurality of captured images and the reflective material. It is something to do.
  • the position / orientation calculation device includes a position information acquisition unit that acquires position information indicating each of a plurality of measurement results by a measuring instrument that measures the position of a reflective material multiple times, and a plurality of imaging images generated by the imager. The position by the measuring instrument from the image information acquisition unit that acquires the image information indicating each of the images, the plurality of position information acquired by the position information acquisition unit, and the plurality of image information acquired by the image information acquisition unit.
  • the information extraction unit that extracts the information pair that is a set of position information and image information, and the information pair extracted by the information extraction unit, in which the measurement timing of is synchronized with the imaging timing of the object to be measured by the imager. It is provided with a position / attitude calculation unit for calculating each of the position of the moving body and the posture of the moving body.
  • the posture of a moving body can be calculated without mounting a posture detector.
  • FIG. 6 is a hardware configuration diagram of a computer when the arithmetic unit 8 is realized by software, firmware, or the like.
  • FIG. It is explanatory drawing which shows the right-eye camera coordinate system, the normalized image coordinate system, the digital image coordinate system, and the world coordinate system. It is a flowchart which shows the processing procedure of the arithmetic unit 8 including the position attitude calculation apparatus 100 which concerns on Embodiment 1.
  • FIG. It is a flowchart which shows the processing procedure of the position-posture calculation unit 14 in the position-posture calculation device 100.
  • It is explanatory drawing which shows the measurement interval of the position of the reflective material 3 by the measuring instrument 2 and the imaging interval of the object of measurement by the image pickup device 4.
  • It is explanatory drawing which shows the camera shooting timing when the moving body 1 is stationary, and the camera shooting timing when the moving body 1 is moving.
  • FIG. 1 is a perspective view showing a main part of a surveying device 200 including a position / posture calculation device 100 according to the first embodiment.
  • FIG. 2 is a block diagram showing a surveying device 200 including a position / posture calculating device 100 according to the first embodiment.
  • FIG. 3 is a block diagram showing an arithmetic unit 8 including a position / posture calculation device 100 according to the first embodiment.
  • FIG. 4 is a hardware configuration diagram showing the hardware of the arithmetic unit 8 including the position / posture calculation device 100 according to the first embodiment.
  • the surveying device 200 includes a moving body 1 and a measuring instrument 2.
  • the moving body 1 is realized by, for example, a dolly.
  • the moving body 1 is realized by a dolly.
  • the moving body 1 may be realized by, for example, a vehicle, an aircraft, a ship, or a backpack.
  • the measuring instrument 2 is realized by, for example, an automatic tracking type total station, and has a function of tracking the reflective material 3.
  • the measuring instrument 2 measures the position of the reflective material 3 provided on the moving body 1 a plurality of times, and outputs the position information indicating each of the plurality of measurement results to the storage device 7 described later. That is, the measuring instrument 2 irradiates the light for position measurement toward the reflective material 3 provided on the moving body 1.
  • the measuring instrument 2 receives the light reflected by the reflective material 3.
  • the measuring instrument 2 calculates the position of the reflective material 3 from, for example, the time required from the irradiation of the light to the reception of the light and the irradiation direction of the light.
  • the measuring instrument 2 is realized by an automatic tracking type total station. However, this is only an example, and when the moving body 1 is stationary at a plurality of timings, the measuring instrument 2 may be realized by a normal total station having no automatic tracking function.
  • the reflective material 3 is realized by, for example, an all-around prism.
  • the reflective material 3 is provided on the moving body 1 and reflects the light emitted from the measuring instrument 2.
  • the reflective material 3 is realized by an all-around prism.
  • this is only an example, and if the amount of change in the relative direction of the moving body 1 with respect to the measuring instrument 2 is small, a normal prism can be used as the reflective material 3.
  • the imager 4 is realized, for example, by a stereo camera having both a right-eye camera 5 and a left-eye camera 6.
  • the imager 4 is mounted on the moving body 1.
  • the imager 4 images the surveyed object a plurality of times and generates a plurality of captured images.
  • the imager 4 outputs image information indicating each captured image to the storage device 7.
  • the imager 4 captures an arbitrary area (hereinafter referred to as an "imaging target area").
  • the image pickup target area is, for example, a region including a survey target object by the surveying apparatus 200.
  • the object to be surveyed is, for example, the ground or a structure.
  • each of the right-eye camera 5 and the left-eye camera 6 is realized by a digital camera.
  • the field of view of the right eye camera 5 overlaps with the field of view of the left eye camera 6.
  • the image captured by the right eye camera 5 is referred to as a “right eye image”.
  • the image captured by the left eye camera 6 is referred to as a "left eye image”.
  • the right eye image and the left eye image may be collectively referred to as a "digital image”.
  • the storage device 7 is realized by, for example, an HDD (Hard Disk Drive) or an SSD (Solid State Drive). In FIG. 1, the description of the storage device 7 is omitted.
  • the storage device 7 stores both the position information output from the measuring device 2 and the image information output from the image pickup device 4.
  • the arithmetic unit 8 includes a position / posture calculation device 100 and a three-dimensional shape calculation unit 15. In FIG. 1, the description of the arithmetic unit 8 is omitted.
  • the position / posture calculation device 100 includes a position information acquisition unit 11, an image information acquisition unit 12, an information extraction unit 13, and a position / orientation calculation unit 14.
  • the position / posture calculation device 100 is a device that calculates the position of the moving body 1 including the image pickup device 4 and the reflective material 3 and the posture of the moving body 1.
  • the position information acquisition unit 11 is realized by, for example, the position information acquisition circuit 21 shown in FIG.
  • the position information acquisition unit 11 acquires position information indicating each of the plurality of measurement results by the measuring device 2 from the storage device 7.
  • the position information acquisition unit 11 outputs each acquired position information to the information extraction unit 13.
  • the image information acquisition unit 12 is realized by, for example, the image information acquisition circuit 22 shown in FIG.
  • the image information acquisition unit 12 acquires image information indicating each image captured by the imager 4 from the storage device 7.
  • the image information acquisition unit 12 outputs each acquired image information to the information extraction unit 13.
  • the information extraction unit 13 is realized by, for example, the information extraction circuit 23 shown in FIG.
  • the information extraction unit 13 determines the position measurement timing by the measuring instrument 2 and the imager 4 from the plurality of position information acquired by the position information acquisition unit 11 and the plurality of image information acquired by the image information acquisition unit 12.
  • An information pair that is a set of position information and image information that is synchronized with the imaging timing of the object to be measured is extracted.
  • the information extraction unit 13 outputs the extracted information pair to the position / orientation calculation unit 14, and outputs the image information included in the information pair to the three-dimensional shape calculation unit 15.
  • the position / attitude calculation unit 14 is realized by, for example, the position / attitude calculation circuit 24 shown in FIG.
  • the position / orientation calculation unit 14 calculates the position of the moving body 1 and the posture of the moving body 1 from the information pair extracted by the information extraction unit 13.
  • the position / attitude calculation unit 14 outputs each of the position information indicating the position of the moving body 1 and the posture information indicating the posture of the moving body 1 to the three-dimensional shape calculation unit 15.
  • the position of the moving body 1 calculated by the position / orientation calculation unit 14 corresponds to the position of the imager 4
  • the posture of the moving body 1 calculated by the position / attitude calculation unit 14 Corresponds to the posture of the imager 4.
  • the three-dimensional shape calculation unit 15 is realized by, for example, the three-dimensional shape calculation circuit 25 shown in FIG.
  • the three-dimensional shape calculation unit 15 acquires the image information included in the information pair extracted by the information extraction unit 13.
  • the three-dimensional shape calculation unit 15 acquires position information indicating the position of the moving body 1 and posture information indicating the posture of the moving body 1 from the position / orientation calculation unit 14.
  • the three-dimensional shape calculation unit 15 calculates the three-dimensional shape of the object to be surveyed by using the captured image indicated by the image information, the position of the moving body 1 indicated by the position information, and the posture of the moving body 1 indicated by the posture information. do.
  • the three-dimensional shape calculation unit 15 calculates the three-dimensional shape of the surveyed object by using, for example, a so-called “multi-viewpoint stereo” technique. Since the "multi-viewpoint stereo” technique itself is a known technique, detailed description thereof will be omitted.
  • each of the position information acquisition unit 11, the image information acquisition unit 12, the information extraction unit 13, the position / orientation calculation unit 14, and the three-dimensional shape calculation unit 15, which are the constituent elements of the arithmetic unit 8, is shown in FIG. It is supposed to be realized by dedicated hardware. That is, it is assumed that the arithmetic unit 8 is realized by the position information acquisition circuit 21, the image information acquisition circuit 22, the information extraction circuit 23, the position / orientation calculation circuit 24, and the three-dimensional shape calculation circuit 25.
  • Each of the position information acquisition circuit 21, the image information acquisition circuit 22, the information extraction circuit 23, the position / orientation calculation circuit 24, and the three-dimensional shape calculation circuit 25 is, for example, a single circuit, a composite circuit, a programmed processor, or a parallel programming.
  • the processor, ASIC (Application Specific Integrated Circuit), FPGA (Field-Programmable Gate Array), or a combination thereof is applicable.
  • the components of the arithmetic unit 8 are not limited to those realized by dedicated hardware, and the arithmetic unit 8 may be realized by software, firmware, or a combination of software and firmware. ..
  • the software or firmware is stored as a program in the memory of the computer.
  • a computer means hardware that executes a program, and corresponds to, for example, a CPU (Central Processing Unit), a central processing unit, a processing unit, a computing device, a microprocessor, a microcomputer, a processor, or a DSP (Digital Signal Processor). do.
  • FIG. 5 is a hardware configuration diagram of a computer when the arithmetic unit 8 is realized by software, firmware, or the like.
  • the arithmetic unit 8 is realized by software, firmware, or the like, each processing procedure in the position information acquisition unit 11, the image information acquisition unit 12, the information extraction unit 13, the position / orientation calculation unit 14, and the three-dimensional shape calculation unit 15 is performed.
  • the program to be executed by the computer is stored in the memory 31.
  • the processor 32 of the computer executes the program stored in the memory 31.
  • FIG. 4 shows an example in which each of the components of the arithmetic unit 8 is realized by dedicated hardware
  • FIG. 5 shows an example in which the arithmetic unit 8 is realized by software, firmware, or the like.
  • the coordinate system used by the position / orientation calculation device 100 includes a world coordinate system, a right-eye camera coordinate system, a left-eye camera coordinate system, a moving body coordinate system, a digital image coordinate system, and a normalized image coordinate system.
  • the world coordinate system is a three-dimensional coordinate system of a right-handed system having X-axis, Y-axis, and Z-axis orthogonal to each other.
  • the XY plane in the world coordinate system is parallel to the ground, which is the object of survey, or substantially parallel to the ground.
  • the positive direction of the Z axis in the world coordinate system indicates the zenith direction.
  • the traveling direction of the moving body 1 is, for example, the positive direction of the X axis in the world coordinate system.
  • FIG. 6 is an explanatory diagram showing a right-eye camera coordinate system, a normalized image coordinate system, a digital image coordinate system, and a world coordinate system.
  • the right-handed camera coordinate system is a three-dimensional coordinate system of a right-handed system having X-axis, Y-axis, and Z-axis orthogonal to each other.
  • the origin of the right-eye camera coordinate system corresponds to the projection center of the right-eye camera 5.
  • the XY plane in the right-eye camera coordinate system is parallel to the projection plane of the right-eye camera 5.
  • the positive direction of the X-axis in the right-eye camera coordinate system corresponds to the upward direction on the projection plane of the right-eye camera 5.
  • the positive direction of the Y-axis in the right-eye camera coordinate system corresponds to the left direction on the projection plane of the right-eye camera 5.
  • the left-eye camera coordinate system (not shown in FIG. 6) is a right-handed three-dimensional coordinate system having X-axis, Y-axis, and Z-axis orthogonal to each other, similar to the right-eye camera coordinate system.
  • the origin of the left-eye camera coordinate system corresponds to the projection center of the left-eye camera 6.
  • the XY plane in the left-eye camera coordinate system is parallel to the projection plane of the left-eye camera 6.
  • the positive direction of the X-axis in the left-eye camera coordinate system corresponds to the upward direction on the projection plane of the left-eye camera 6.
  • the positive direction of the Y-axis in the left-eye camera coordinate system corresponds to the left direction on the projection plane of the left-eye camera 6.
  • the moving body coordinate system is any three-dimensional coordinate system having an X-axis, a Y-axis, and a Z-axis orthogonal to each other.
  • the moving body coordinate system is a coordinate system based on the moving body 1.
  • the moving body coordinate system is set according to the application of the surveying device 200 and the like. Specifically, for example, the moving body coordinate system is set to the right-eye camera coordinate system shown in FIG. 6 or the coordinate system that matches the left-eye camera coordinate system. In the surveying apparatus 200 shown in FIG. 1, it is assumed that the moving body coordinate system and the right eye camera coordinate system match.
  • the position of the moving body 1 calculated by the position / posture calculation unit 14 is the position of the right eye camera 5, and the posture of the moving body 1 calculated by the position / posture calculation unit 14 is the posture of the right eye camera 5. ..
  • the upward direction on the projection surface of the right eye camera 5 substantially coincides with the front direction of the moving body 1.
  • the origin of the moving body coordinate system corresponds to the position of the moving body 1. Therefore, the coordinates indicating the position of the origin of the moving body coordinate system in the world coordinate system indicate the position of the moving body 1 in the world coordinate system.
  • each position coordinate in the world coordinate system, the right eye camera coordinate system, the left eye camera coordinate system, or the moving body coordinate system is referred to as "three-dimensional coordinates".
  • each three-dimensional coordinate at a plurality of tie points described later in the world coordinate system may be referred to as a "tie point position”.
  • the position coordinates in the two-dimensional coordinate system along the projection plane of the right-eye camera 5 or the two-dimensional coordinate system along the projection plane of the left-eye camera 6 are referred to as "projection coordinates”.
  • the digital image coordinate system is an image coordinate system having a U-axis and a V-axis orthogonal to each other.
  • the origin in the digital image coordinate system corresponds to the upper left corner of each digital image.
  • the positive direction of the U-axis in the digital image coordinate system corresponds to the right direction in each digital image.
  • the positive direction of the V axis in the digital image coordinate system corresponds to the downward direction in each digital image.
  • the normalized image coordinate system is an image coordinate system having U-axis and V-axis orthogonal to each other.
  • the origin in the normalized image coordinate system corresponds to the center in each digital image.
  • the positive direction of the U-axis in the normalized image coordinate system corresponds to the upward direction in each digital image.
  • the positive direction of the V-axis in the normalized image coordinate system corresponds to the left direction in each digital image.
  • image coordinate each position coordinate in the digital image coordinate system or the normalized image coordinate system
  • image coordinates of each tie point in the digital image coordinate system or the normalized image coordinate system may be referred to as "tie point coordinates".
  • FIG. 7 is a flowchart showing a processing procedure of the arithmetic unit 8 including the position / posture calculation device 100 according to the first embodiment.
  • FIG. 8 is a flowchart showing a processing procedure of the position / posture calculation unit 14 in the position / posture calculation device 100.
  • the moving body 1 moves along the XY plane in the world coordinate system.
  • the moving direction of the moving body 1 is, for example, a positive direction of the X axis in the world coordinate system and a direction orthogonal to each of the Y axis and the Z axis.
  • the moving body 1 is stationary, for example, at regular time intervals, and the movement and the stationary are alternately repeated.
  • the time interval at which the moving body 1 rests may be different from the measurement interval of the position of the reflector 3 by the measuring instrument 2 and the imaging interval of the object to be measured by the imager 4.
  • the measuring instrument 2 measures the position of the reflective material 3 provided on the moving body 1 a plurality of times, and outputs the position information indicating each of the plurality of measurement results to the storage device 7. That is, the measuring instrument 2 irradiates the reflective material 3 with light for position measurement at regular time intervals while tracking the reflective material 3 during the period in which the moving body 1 repeatedly moves and stands still. The measuring instrument 2 receives the light reflected by the reflective material 3. The measuring instrument 2 calculates the position of the reflective material 3 from, for example, the time required from the irradiation of the light to the reception of the light and the irradiation direction of the light.
  • the position measured by the measuring instrument 2 is the three-dimensional coordinates (x pw , y pw , z pw ) of the position of the reflector 3 in the world coordinate system.
  • the imager 4 images the surveyed object a plurality of times, generates a plurality of captured images, and outputs image information indicating each captured image to the storage device 7. That is, the right eye camera 5 of the imager 4 images the image pickup target area including the survey object a plurality of times during the period in which the moving body 1 repeatedly moves and stands still. The right-eye camera 5 outputs image information indicating a digital image, which is each of the captured right-eye images, to the storage device 7.
  • the left-eye camera 6 of the imager 4 images the image pickup target area including the survey object a plurality of times during the period in which the moving body 1 repeatedly moves and stands still. The left-eye camera 6 outputs image information indicating a digital image, which is each of the captured left-eye images, to the storage device 7.
  • FIG. 9 is an explanatory diagram showing the measurement interval of the position of the reflective material 3 by the measuring instrument 2 and the imaging interval of the object to be measured by the imager 4.
  • the circles painted in black indicate the camera shooting timing
  • the camera shooting timing is the timing at which the right-eye camera 5 and the left-eye camera 6 take a picture of the image pickup target area.
  • the shaded ⁇ indicates the TS measurement timing
  • the TS measurement timing is the timing at which the measuring instrument 2 measures the position of the reflective material 3.
  • the ellipses surrounding each of the black-filled ⁇ and the shaded ⁇ indicate the camera shooting timing and the TS measurement timing when the moving body 1 is stationary.
  • ⁇ that is not surrounded by an ellipse and is filled with black is the camera shooting timing when the moving body 1 is moving, and ⁇ that is shaded not surrounded by an ellipse is that the moving body 1 is moving. This is the TS measurement timing when the camera is running.
  • the position information acquisition unit 11 of the position / posture calculation device 100 acquires position information indicating each measurement result by the measuring device 2 from the storage device 7 (step ST1 in FIG. 7).
  • the position information acquisition unit 11 outputs each acquired position information to the information extraction unit 13.
  • the image information acquisition unit 12 of the position / orientation calculation device 100 acquires image information indicating each captured image generated by the image pickup device 4 from the storage device 7 (step ST2 in FIG. 7).
  • the image information acquisition unit 12 outputs each acquired image information to the information extraction unit 13.
  • the information extraction unit 13 of the position / posture calculation device 100 acquires a plurality of position information from the position information acquisition unit 11, and acquires a plurality of image information from the image information acquisition unit 12.
  • the information extraction unit 13 has the position information and the image in which the measurement timing of the position by the measuring instrument 2 and the imaging timing of the surveyed object by the imager 4 are synchronized from the plurality of position information and the plurality of image information.
  • An information pair that is a set with information is extracted (step ST3 in FIG. 7).
  • the information extraction unit 13 outputs the extracted information pair to the position / orientation calculation unit 14.
  • the camera shooting timing by the imager 4 is at regular intervals.
  • the information extraction unit 13 compares a plurality of captured images captured at regular intervals with each other, and calculates the amount of movement between the plurality of captured images based on the comparison result of the captured images.
  • the amount of movement between a plurality of captured images can be determined, for example, by using a method of template matching or optical flow estimation.
  • the information extraction unit 13 determines whether the moving body 1 is stationary or moving between the respective camera shooting timings based on the amount of movement between the plurality of captured images.
  • the information extraction unit 13 compares the movement amount between the plurality of captured images with the movement determination threshold value Th 1 , and if the movement amount is equal to or less than the movement determination threshold value Th 1 , the moving body 1 is stationary. Judge that it is. If the movement amount is larger than the threshold value Th 1 for movement determination, the information extraction unit 13 determines that the moving body 1 is moving.
  • the threshold value Th 1 for movement determination may be stored in the internal memory of the information extraction unit 13, or may be given from the outside of the position / posture calculation device 100.
  • FIG. 10 is an explanatory diagram showing a camera shooting timing when the moving body 1 is stationary and a camera shooting timing when the moving body 1 is moving.
  • the camera shooting timing surrounded by the ellipse is the camera shooting timing when the moving body 1 is stationary. That is, even if the moving body 1 moves slightly, the moving speed is low, so this is the camera shooting timing when the moving body 1 is treated as if it is stationary.
  • the camera shooting timing not surrounded by the ellipse is the camera shooting timing when the moving body 1 is moving. In the example of FIG. 10, nine consecutive camera shooting timings are determined as camera shooting timings when the moving body 1 is stationary. As shown in FIG.
  • the TS measurement timing by the measuring instrument 2 is at regular intervals.
  • the information extraction unit 13 compares a plurality of positions calculated at regular intervals with each other, and calculates the amount of movement between the plurality of positions based on the comparison result of the positions.
  • the amount of movement between a plurality of positions can be obtained, for example, as the Euclidean distance.
  • the information extraction unit 13 determines whether the moving body 1 is stationary or moving between the respective TS measurement timings based on the amount of movement between the plurality of positions. That is, the information extraction unit 13 compares the amount of movement between a plurality of positions with the threshold value Th 2 for movement determination, and if the movement amount is equal to or less than the threshold value Th 2 for movement determination, the moving body 1 stands still.
  • the information extraction unit 13 determines that the moving body 1 is moving.
  • the threshold value Th 2 for movement determination may be stored in the internal memory of the information extraction unit 13, or may be given from the outside of the position / posture calculation device 100.
  • the information extraction unit 13 assumes that the position related to the TS measurement timing in which the position calculation time is intermediate is the stationary position. The information extraction unit 13 discards a position related to the TS measurement timing whose position calculation time is not in the middle among the continuous TS measurement timings.
  • the information extraction unit 13 measures the position measurement timing from the position information indicating each of the plurality of positions remaining without being discarded and the image information indicating each of the plurality of captured images remaining without being discarded. And the pair of the position information and the image information in which the imaging timing of the surveyed object is synchronized with each other is extracted.
  • the positions that remain without discarding include the positions that remain without discarding, when they are stationary, and the positions when they are moving.
  • the captured image remaining without being discarded includes a captured image when it is stationary and a captured image when it is moving, which remains without being discarded.
  • the camera shooting timing of the first captured image from the left side in the figure is the TS measurement timing of the first position from the left side among the TS measurement timings of a plurality of positions remaining without being discarded. And the closest.
  • the TS measurement timing closest to the camera shooting timing is treated as being synchronized not only when it completely coincides with the camera shooting timing but also when it deviates from the camera shooting timing.
  • the difference between the camera shooting timing and the TS measurement timing is at most half of the camera shooting timing interval, and the difference between the camera shooting timing and the TS measurement timing is larger than the camera shooting timing interval.
  • the synchronization between the measurement timing and the imaging timing here is not limited to the one in which the measurement timing time and the camera imaging timing time completely match, and the measurement timing time is within a range where there is no practical problem. And the time of the camera imaging timing is different.
  • the information extraction unit 13 assumes that the camera shooting timing of the first captured image from the left side and the TS measurement timing of the first position from the left side are synchronized with the position information indicating the first position from the left side and the left side.
  • the pair with the image information indicating the first captured image is used as an information pair.
  • the camera shooting timing of the second captured image from the left side is the closest to the TS measurement timing of the second position from the left side among the TS measurement timings of the plurality of positions remaining without being discarded.
  • the information extraction unit 13 assumes that the camera shooting timing of the second captured image from the left side and the TS measurement timing of the second position from the left side are synchronized, and the position information indicating the second position from the left side and the left side The pair with the image information indicating the second captured image is used as an information pair.
  • the camera shooting timing of the third captured image from the left side is the closest to the TS measurement timing of the second position from the left side among the TS measurement timings of the plurality of positions remaining without being discarded.
  • the information extraction unit 13 assumes that the camera shooting timing of the third captured image from the left side and the TS measurement timing of the second position from the left side are synchronized with the position information indicating the second position from the left side and the left side.
  • the pair with the image information indicating the third captured image is used as an information pair.
  • the camera shooting timing of the fourth captured image from the left side is the closest to the TS measurement timing of the third position from the left side among the TS measurement timings of the plurality of positions remaining without being discarded.
  • the information extraction unit 13 assumes that the camera shooting timing of the fourth captured image from the left side and the TS measurement timing of the third position from the left side are synchronized with the position information indicating the third position from the left side and the left side.
  • the pair with the image information indicating the fourth captured image is used as an information pair.
  • the camera shooting timing of the fifth captured image from the left side is the closest to the TS measurement timing of the fourth position from the left side among the TS measurement timings of the plurality of positions remaining without being discarded.
  • the information extraction unit 13 assumes that the camera shooting timing of the fifth captured image from the left side and the TS measurement timing of the fourth position from the left side are synchronized with the position information indicating the fourth position from the left side and the left side.
  • the pair with the image information indicating the fifth captured image is used as an information pair.
  • the position / posture calculation unit 14 acquires a plurality of information pairs from the information extraction unit 13.
  • the position / attitude calculation unit 14 calculates the position of the moving body 1 and the posture of the moving body 1 from each information pair (step ST4 in FIG. 7).
  • the position / attitude calculation unit 14 outputs each of the position information indicating the position of the moving body 1 and the posture information indicating the posture of the moving body 1 to the three-dimensional shape calculation unit 15.
  • the position and posture calculation process by the position / posture calculation unit 14 will be specifically described.
  • the points on the digital image corresponding to each of the plurality of points in the image pickup target area are set as tie points.
  • the position of each point may be known or unknown in the surveying apparatus 200.
  • Each point may be the position of a marker provided in the image pickup target area.
  • the point may be referred to as an "unknown point”.
  • the marker may be referred to as a "known marker”.
  • the marker may be referred to as an "unknown marker".
  • the position / orientation calculation unit 14 acquires a plurality of information pairs from the information extraction unit 13.
  • the position / orientation calculation unit 14 calculates the image coordinates (u, v) of each tie point in the digital image which is the captured image indicated by the image information included in each information pair (step ST11 in FIG. 8). That is, if each tie point corresponds to an unknown point, the position / orientation calculation unit 14 executes image processing for each digital image, so that the feature is commonly included in the plurality of digital images. Extract points.
  • the position / orientation calculation unit 14 calculates the image coordinates (u, v) of the extracted feature points, assuming that the extracted feature points are tie points. Since the feature point extraction process itself is a known technique, detailed description thereof will be omitted.
  • the feature point extraction process is described in, for example, Reference 1 below. [Reference 1] David G. Lowe, "Distinctive Image Features from Scale-Invariant Keypoints," International Journal of Computer Vision, 2004, 60 (2), pp. 91-110.
  • the position / orientation calculation unit 14 performs template matching for each digital image, so that the marker included in each digital image is included. Is detected.
  • the position / posture calculation unit 14 calculates the image coordinates (u, v) of the detected marker, assuming that the detected marker is a tie point. Further, when each tie point corresponds to a known marker or an unknown marker, the user of the surveying apparatus 200 visually confirms the position of the marker included in each digital image, and the user can check the position of the marker. The confirmed position may be given to the position / orientation calculation unit 14.
  • the position / posture calculation unit 14 calculates the image coordinates (u, v) corresponding to the position given by the user.
  • the position / orientation calculation unit 14 calculates the initial values of the three-dimensional coordinates (x T , y T , z T ) of each tie point in the world coordinate system (step ST12 in FIG. 8).
  • the position / posture calculation unit 14 calculates the average value of all the X coordinates (x pw ) of the positions indicated by the position information included in the plurality of information pairs.
  • the position / posture calculation unit 14 uses the average value of the X coordinate (x pw ) as the initial value of the X coordinate (x T ) of each tie point.
  • the position / posture calculation unit 14 calculates the average value of all the Y coordinates (y pw ) of the positions indicated by the position information included in the plurality of information pairs.
  • the position / posture calculation unit 14 uses the average value of the Y coordinates (y pw ) as the initial value of the Y coordinates (y T ) of each tie point.
  • the position / posture calculation unit 14 calculates the Z coordinate of an arbitrary point on the ground by using the Z coordinate (z pw ) of the position indicated by the position information included in the plurality of information pairs.
  • the Z coordinate of any point on the ground is the Z coordinate of a position close to the height of the ground.
  • the position / orientation calculation unit 14 calculates the average value of all Z coordinates (z pw ) of the positions indicated by the position information included in the plurality of information pairs. Then, the position / orientation calculation unit 14 subtracts the known Z coordinate indicating the height of the position where the reflector 3 is installed with respect to the moving body 1 from the average value of the Z coordinate (z pw ). Calculate the Z coordinate of any point on the ground.
  • the position / attitude calculation unit 14 uses the Z coordinate of an arbitrary point on the ground as the initial value of the Z coordinate (z T ) of each tie point.
  • the position / posture calculation unit 14 calculates the initial values of the posture angles ( ⁇ rw , ⁇ rw , ⁇ rw ) of the right eye camera 5 (step ST13 in FIG. 8).
  • the process of calculating the initial value of the posture angle ( ⁇ rw , ⁇ rw , ⁇ rw ) by the position / posture calculation unit 14 will be specifically described.
  • the position / orientation calculation unit 14 it is assumed that the three-dimensional coordinates of the reflector 3 at time t are (x, y, z). It is assumed that the time corresponding to the TS measurement timing next to the TS measurement timing corresponding to the time t is t'.
  • the traveling direction of the moving body 1 is set to be the same as or substantially the same as the front direction of the moving body 1.
  • the posture angle ( ⁇ rw , ⁇ rw , ⁇ rw ) of the right-eye camera 5 uses the X coordinate (1,0,0) as the movement vector (x-x', y-y', z-) of the reflector 3. It is approximated by the angle of rotation that matches z').
  • the right-eye camera 5 moves along the XY plane in the world coordinate system. Therefore, each of the roll angle ( ⁇ rw ) and the pitch angle ( ⁇ rw ) is approximated to 0. Further, the yaw angle ( ⁇ rw ) is approximated by the angle of rotation that makes the vector (1,0) in the XY plane coincide with the vector (xx', yy'). Therefore, the following equation (3) holds for the yaw angle ( ⁇ rw ).
  • the position / posture calculation unit 14 calculates the yaw angle ( ⁇ rw ) by the equation (3).
  • the yaw angle ( ⁇ rw ) is calculated by the inverse trigonometric function.
  • the initial value of the attitude angle ( ⁇ rw , ⁇ rw , ⁇ rw ) at time t ” is used as the initial value of the attitude angle ( ⁇ rw , ⁇ rw , ⁇ rw ) at time t.
  • the initial value of the attitude angle ( ⁇ rw , ⁇ rw , ⁇ rw ) at time t ” is calculated using the three-dimensional coordinates of the reflector 3 at time t and the three-dimensional coordinates of the reflector 3 at time t”.
  • the position / orientation calculation unit 14 calculates the initial values of the three-dimensional coordinates (x rw , y rw , z rw ) of the position of the right eye camera 5 (step ST14 in FIG. 8).
  • the process of calculating the initial values of the three-dimensional coordinates (x rw , y rw , z rw ) by the position / orientation calculation unit 14 will be specifically described.
  • the position / orientation calculation unit 14 it is assumed that the three-dimensional coordinates of the position of the reflector 3 in the world coordinate system are (x pw , y pw , z pw ).
  • the three-dimensional coordinates of the position of the reflector 3 in the right-eye camera coordinate system are (x pr , y pr , z pr ).
  • the attitude angle of the right eye camera 5 in the world coordinate system is ( ⁇ rw , ⁇ rw , ⁇ rw ).
  • the three-dimensional coordinates (x rw , y rw , z rw ) of the position of the right eye camera 5 are expressed by the following equations (4) and (5).
  • the position / orientation calculation unit 14 substitutes the initial values of the attitude angles ( ⁇ rw , ⁇ rw , ⁇ rw ) of the right eye camera 5 calculated in step ST13 into the equation (5), and based on the equation (4), right.
  • the initial values of the three-dimensional coordinates (x rw , y rw , z rw ) of the position of the eye camera 5 are calculated.
  • the position / orientation calculation unit 14 brings the initial value of the three-dimensional coordinates (x T , y T , z T ) of the tie point closer to the actual value, and the attitude angle ( ⁇ rw , ⁇ rw , of the right eye camera 5) of the right eye camera 5 is set.
  • Optimize processing is performed to bring the initial value of ⁇ rw ) closer to the actual value and to bring the initial value of the three-dimensional coordinates (x rw , y rw , z rw ) of the position of the right-eye camera 5 closer to the actual value (Fig.).
  • Step 8 ST15 the optimization process by the position / orientation calculation unit 14 will be specifically described.
  • the position / orientation calculation unit 14 calculates the projected coordinates ( 0 ur , 0 vr ) of each tie point in the right eye camera 5 as shown below. Further, the position / orientation calculation unit 14 calculates the projected coordinates ( 0 u l , 0 v l ) of each tie point in the left eye camera 6. These projected coordinates ( 0 ur , 0 vr ), ( 0 ul , 0 vr ) are the position coordinates in the normalized image coordinate system.
  • the focal length of the right eye camera 5 is fr
  • the width of the image sensor in the right eye camera 5 is Cr
  • the width of the right eye image is W r .
  • the conversion formula for converting the three-dimensional coordinates X w into the projection coordinates ( 0 ur, 0 vr ) is expressed by the following formulas (7) and (8).
  • T is a mathematical symbol representing transposition.
  • the position / orientation calculation unit 14 determines the projected coordinates ( 0 ur ) in the right-eye camera 5 according to the equations (7) and (8) for the tie points having the three-dimensional coordinates corresponding to the three-dimensional coordinates X w in the world coordinate system. , 0 vr ) is calculated.
  • the position / orientation calculation unit 14 also has the projected coordinates ( 0 ul, 0 v l ) of each tie point in the left eye camera 6 as well as the projected coordinates ( 0 ur , 0 v) of each tie point in the right eye camera 5 . Calculate in the same way as r ). Specifically, it is assumed that the three-dimensional coordinates of the origin of the left-eye camera coordinate system in the world coordinate system are (x lw , y lw , z lw ). Further, it is assumed that the rotation angle of the coordinate axes of the left-eye camera coordinate system with respect to the coordinate axes of the world coordinate system is ( ⁇ lw , ⁇ lw , ⁇ lw ).
  • the position / orientation calculation unit 14 uses a conversion formula for converting the three-dimensional coordinates X w into the projected coordinates ( 0 u l , 0 v l ), and uses the projected coordinates ( 0 u l ) in the left eye camera 6. , 0 v l ) is calculated.
  • the three-dimensional coordinates (x lw , y lw , z lw ) and the rotation angles ( ⁇ lw , ⁇ lw , ⁇ lw ) are the three-dimensional coordinates (x rw , y) of the origin of the right-eye camera coordinate system in the world coordinate system.
  • rw , z rw rotation angle of the coordinate axis of the right eye camera coordinate system with respect to the coordinate axis of the world coordinate system ( ⁇ rw , ⁇ rw , ⁇ rw ), three-dimensional coordinates of the origin of the left eye camera coordinate system in the right eye camera coordinate system.
  • the position / orientation calculation unit 14 corrects the distortion in the image coordinates (u, v) of the tie point calculated in step ST11, as shown below.
  • the image coordinates of the tie points in the digital image coordinate system are ( 1 ur , 1 v r ) in the right eye image.
  • the position coordinates indicating the principal point position of the right eye camera 5 are (X pr , Y pr ), and the distortion coefficients in the right eye camera 5 are k 1r , k 2r , k 3r , p 1r , p 2r , b 1r and b. It is assumed that it is 2r .
  • the width of the image pickup device in the right eye camera 5 is Cr
  • the horizontal width of the right eye image is Wr
  • the vertical width of the right eye image is Hr .
  • the coordinates of the tie point having distortion in the digital image coordinate system ( 1 ur , 1 vr ) are changed to the coordinates of the tie point after distortion correction in the normalized image coordinate system ( 2 ur , 2 vr ).
  • the conversion formulas to be converted are represented by the following formulas (13) to (19).
  • the position / orientation calculation unit 14 obtains the coordinates ( 1 ur, 1 vr ) of the tie point having the distortion in the digital image coordinate system after the distortion correction in the normalized image coordinate system according to the equations (13) to (19) . Convert to the coordinates of the tie point ( 2 ur , 2 vr ). Further, the position / orientation calculation unit 14 determines the coordinates ( 1 ul , 1 v l ) of the tie point having the distortion in the digital image coordinate system after the distortion correction in the normalized image coordinate system, as in the above conversion. Convert to the coordinates of the point ( 2 u l , 2 v l ).
  • the image coordinates of the tie points in the digital image coordinate system in the left eye image are ( 1 u l , 1 v l ).
  • the position coordinates indicating the principal point position of the left-eye camera 6 are (X pl , Y pl ), and the distortion coefficients in the left-eye camera 6 are k 1l , k 2l , k 3l , p 1l , p 2l , b 1l and b. It is assumed that it is 2 liters .
  • the position / orientation calculation unit 14 uses the same conversion formulas as those in the formulas (13) to (19) to obtain the coordinates ( 1 ul , 1 v l ) of the tie point having the distortion in the digital image coordinate system. , Converts to the coordinates of the tie point after distortion correction in the normalized image coordinate system ( 2 ul , 2 v l ).
  • the position / orientation calculation unit 14 calculates the difference value between the calculated projected coordinates ( 0 ur , 0 vr ) of the right eye camera 5 and the calculated tie point coordinates ( 2 ur, 2 vr ) . Minimize. Further, the position / orientation calculation unit 14 minimizes the difference value between the calculated projected coordinates ( 0 ul , 0 v l ) of the left eye camera 6 and the calculated tie point coordinates ( 2 ul , 2 v l ). To become.
  • the parameters to be optimized are the three-dimensional coordinates (x T , y T , z T ) of each tie point in the world coordinate system, the posture angle of the right eye camera 5 ( ⁇ rw , ⁇ rw , ⁇ rw ), and the right eye. These are the three-dimensional coordinates (x rw , y rw , z rw ) of the position of the camera 5. All parameters other than the parameters to be optimized are constants. Alternatively, the parameters other than the parameters to be optimized are calculated using the parameters to be optimized.
  • the three-dimensional coordinates (x rw , y rw , z rw ) of the position of the right-eye camera 5 are included in the parameters to be optimized, but the right when the moving body 1 is stationary.
  • the three-dimensional coordinates (x rw , y rw , z rw ) of the position of the eye camera 5 are not included in the parameters to be optimized. That is, the position / orientation calculation unit 14 does not obtain the three-dimensional coordinates (x rw , y rw , z rw ) of the position of the right eye camera 5 when the moving body 1 is stationary by the optimization process.
  • the position / orientation calculation unit 14 has three-dimensional coordinates (x pw , y pw , z pw ) of the position of the reflective material 3 in the world coordinate system, and the position of the reflective material 3 in the right eye camera coordinate system.
  • the three-dimensional coordinates (x pr , y pr , z pr ) and the attitude angle ( ⁇ rw , ⁇ rw , ⁇ rw ) of the right-eye camera 5 in the world coordinate system the three-dimensional coordinates (x rw , y rw ,) z rw ) is calculated.
  • step ST15 is a nonlinear least squares problem that minimizes the sum of squares of the difference values of the image coordinate values, and various known techniques can be used for the solution. Detailed description of these techniques will be omitted. Specifically, for example, the technique described in Reference 2 below can be used. [Reference 2] Levenberg, Kenneth. "A method for the solution of certain non-linear problems in least squares.” Quarterly of applied mathematics 2.2 (1944): 164-168.
  • the position of the right-eye camera 5 and the posture of the right-eye camera 5 can be calculated with high accuracy.
  • optimization of 6 degrees of freedom is performed in which each of the camera position and orientation is unknown.
  • the right eye camera 5 is optimized by optimizing only the posture of the right eye camera 5 when the moving body 1 is stationary as an unknown number. The position of the right eye camera 5 and the posture of the right eye camera 5 can be calculated with high accuracy.
  • the three-dimensional shape calculation unit 15 acquires the image information included in the information pair extracted by the information extraction unit 13.
  • the three-dimensional shape calculation unit 15 acquires position information indicating the position of the moving body 1 and posture information indicating the posture of the moving body 1 from the position / orientation calculation unit 14.
  • the position of the moving body 1 corresponds to the position of the right eye camera 5
  • the posture of the moving body 1 corresponds to the posture of the right eye camera 5.
  • the position of the right-eye camera 5 is three-dimensional coordinates (x rw , y rw , z rw )
  • the posture of the right-eye camera 5 is the posture angle ( ⁇ rw , ⁇ rw , ⁇ rw ).
  • the three-dimensional shape calculation unit 15 calculates the three-dimensional shape of the object to be surveyed by using the captured image indicated by the image information, the position of the moving body 1 indicated by the position information, and the posture of the moving body 1 indicated by the posture information. (Step ST5 in FIG. 7).
  • the three-dimensional shape calculation unit 15 calculates the three-dimensional shape of the surveyed object by using, for example, a so-called “multi-viewpoint stereo” technique. Since each of the position of the right eye camera 5 and the posture of the right eye camera 5 is obtained with high accuracy by the position / orientation calculation unit 14, the calculation accuracy of the three-dimensional shape by the three-dimensional shape calculation unit 15 is improved.
  • the three-dimensional shape of the surveyed object can be calculated without using a sensor for posture detection.
  • the position information acquisition unit 11 and the imager 4 acquire position information indicating each of a plurality of measurement results by the measuring device 2 that measures the position of the reflective material 3 a plurality of times.
  • An image information acquisition unit 12 that acquires image information indicating each of the generated captured images, a plurality of position information acquired by the position information acquisition unit 11, and a plurality of image information acquired by the image information acquisition unit 12.
  • an information extraction unit 13 that extracts an information pair that is a set of position information and image information in which the measurement timing of the position by the measuring instrument 2 and the imaging timing of the object to be measured by the imager 4 are synchronized
  • the position / orientation calculation device 100 is configured to include a position / orientation calculation unit 14 that calculates each of the position of the moving body 1 and the posture of the moving body 1 from the information pair extracted by the information extraction unit 13. Therefore, the position / posture calculation device 100 can calculate the posture of the moving body 1 without mounting the posture detector.
  • the imager 4 is realized by a stereo camera having both a right-eye camera 5 and a left-eye camera 6.
  • the imager 4 may be any one on which one or more digital cameras are mounted. By increasing the number of digital cameras mounted on the imager 4, it is possible to further improve the estimation accuracy of each of the positions and postures of the moving body 1. By reducing the number of digital cameras mounted on the imager 4, the surveying apparatus 200 can be realized at low cost.
  • the area to be imaged is not limited to the area including the object to be surveyed.
  • the object to be surveyed is not limited to the ground.
  • the survey object may be the surface of a bridge or the inner surface of a tunnel. That is, the surveyed object may include a horizontal plane, a vertical plane, or an inclined surface. Further, the object to be surveyed may be a flat surface, a curved surface, or an uneven surface.
  • the moving body 1 may be provided with, for example, a laser scanner as a measuring instrument for distance measurement. ..
  • the three-dimensional shape calculation unit 15 calculates the three-dimensional shape of the object to be measured by using the distance measured by the measuring instrument for distance measurement and the position and posture calculated by the position / attitude calculation unit 14, respectively. May be.
  • the position / posture calculation device includes a position information acquisition unit, an image information acquisition unit, an information extraction unit, and a position / attitude calculation unit, and can calculate the posture of a moving body without mounting a posture detector. It can be used and is suitable for calculating the posture of a moving body.

Landscapes

  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

測量対象物を複数回撮像し、複数の撮像画像を生成する撮像器（４）と、反射材（３）とを備えている移動体（１）の位置及び移動体（１）の姿勢のそれぞれを算出する位置姿勢算出装置（１００）であって、反射材（３）の位置を複数回測定する測定器（２）による複数の測定結果のそれぞれを示す位置情報を取得する位置情報取得部（１１）と、撮像器（４）により生成されたそれぞれの撮像画像を示す画像情報を取得する画像情報取得部（１２）と、位置情報取得部（１１）により取得された複数の位置情報と画像情報取得部（１２）により取得された複数の画像情報との中から、測定器（２）による位置の測定タイミングと撮像器（４）による測量対象物の撮像タイミングとが同期している、位置情報と画像情報との組である情報対を抽出する情報抽出部（１３）と、情報抽出部（１３）により抽出された情報対から、移動体（１）の位置及び移動体（１）の姿勢のそれぞれを算出する位置姿勢算出部（１４）とを備えるように、位置姿勢算出装置（１００）を構成した。

Description

位置姿勢算出装置、位置姿勢算出方法及び測量装置
　本開示は、位置姿勢算出装置、位置姿勢算出方法及び測量装置に関するものである。
　以下の特許文献１には、測量対象物の三次元形状を測量する測量装置が開示されている。当該測量装置は、測量対象物を撮像する撮像器と、撮像器を搭載している移動体の位置を測定する測定器と、当該移動体の姿勢を検出する姿勢検出器とを備えている。当該測量装置は、撮像器による撮像画像と、測定器により測定された位置と、姿勢検出器により検出された姿勢とを用いて、測量対象物の三次元形状を算出している。
特開２０１７－２０９７２号公報
　特許文献１に開示されている測量装置において、三次元形状の高精度な算出を実現するには、当該測量装置が、移動体の姿勢を高精度に検出することが可能な姿勢検出器を実装している必要がある。しかしながら、姿勢を高精度に検出することが可能な姿勢検出器は、一般的に高価である。
　本開示は、上記のような課題を解決するためになされたもので、姿勢検出器を実装することなく、移動体の姿勢を算出することができる位置姿勢算出装置及び位置姿勢算出方法を得ることを目的とする。
　本開示に係る位置姿勢算出装置は、測量対象物を複数回撮像し、複数の撮像画像を生成する撮像器と、反射材とを備えている移動体の位置及び移動体の姿勢のそれぞれを算出するものである。本開示に係る位置姿勢算出装置は、反射材の位置を複数回測定する測定器による複数の測定結果のそれぞれを示す位置情報を取得する位置情報取得部と、撮像器により生成された複数の撮像画像のそれぞれを示す画像情報を取得する画像情報取得部と、位置情報取得部により取得された複数の位置情報と画像情報取得部により取得された複数の画像情報との中から、測定器による位置の測定タイミングと撮像器による測量対象物の撮像タイミングとが同期している、位置情報と画像情報との組である情報対を抽出する情報抽出部と、情報抽出部により抽出された情報対から、移動体の位置及び移動体の姿勢のそれぞれを算出する位置姿勢算出部とを備えるものである。
　本開示によれば、姿勢検出器を実装することなく、移動体の姿勢を算出することができる。
実施の形態１に係る位置姿勢算出装置１００を含む測量装置２００の要部を示す斜視図である。 実施の形態１に係る位置姿勢算出装置１００を含む測量装置２００を示すブロック図である。 実施の形態１に係る位置姿勢算出装置１００を含む演算装置８を示すブロック図である。 実施の形態１に係る位置姿勢算出装置１００を含む演算装置８のハードウェアを示すハードウェア構成図である。 演算装置８が、ソフトウェア又はファームウェア等によって実現される場合のコンピュータのハードウェア構成図である。 右眼カメラ座標系、正規化画像座標系、デジタル画像座標系及び世界座標系を示す説明図である。 実施の形態１に係る位置姿勢算出装置１００を含む演算装置８の処理手順を示すフローチャートである。 位置姿勢算出装置１００における位置姿勢算出部１４の処理手順を示すフローチャートである。 測定器２による反射材３の位置の測定間隔及び撮像器４による測定対象物の撮像間隔を示す説明図である。 移動体１が静止しているときのカメラ撮影タイミング及び移動体１が移動しているときのカメラ撮影タイミングを示す説明図である。
　以下、本開示をより詳細に説明するために、本開示を実施するための形態について、添付の図面に従って説明する。
実施の形態１．
　図１は、実施の形態１に係る位置姿勢算出装置１００を含む測量装置２００の要部を示す斜視図である。図２は、実施の形態１に係る位置姿勢算出装置１００を含む測量装置２００を示すブロック図である。
　図３は、実施の形態１に係る位置姿勢算出装置１００を含む演算装置８を示すブロック図である。図４は、実施の形態１に係る位置姿勢算出装置１００を含む演算装置８のハードウェアを示すハードウェア構成図である。
　測量装置２００は、移動体１及び測定器２を含んでいる。
　移動体１は、例えば、台車によって実現される。
　図１に示す測量装置２００では、移動体１が台車によって実現されている。しかし、これは一例に過ぎず、移動体１が、例えば、車両、航空機、船舶又は背負子によって実現されるものであってもよい。
　測定器２は、例えば、自動追尾型のトータルステーションによって実現され、反射材３を追尾する機能を有している。
　測定器２は、移動体１に設けられている反射材３の位置を複数回測定し、複数の測定結果のそれぞれを示す位置情報を後述する記憶装置７に出力する。
　即ち、測定器２は、移動体１に設けられている反射材３に向けて、位置測定用の光を照射する。
　測定器２は、反射材３によって反射された光を受信する。
　測定器２は、例えば、光を照射してから光を受信するまでに要した時間と、光の照射方向とから、反射材３の位置を算出する。測定器２による位置の算出処理自体は、公知の技術であるため詳細な説明を省略する。
　図１に示す測量装置２００では、測定器２が自動追尾型のトータルステーションによって実現されている。しかし、これは一例に過ぎず、複数のタイミングで移動体１が静止するものである場合、測定器２が、自動追尾機能を有しない通常のトータルステーションによって実現されるものであってもよい。
　反射材３は、例えば、全周型のプリズムによって実現される。
　反射材３は、移動体１に設けられており、測定器２から照射された光を反射させるものである。
　図１に示す測量装置２００では、反射材３が全周型のプリズムによって実現されている。しかし、これは一例に過ぎず、測定器２に対する移動体１の相対方向の変動量が小さければ、反射材３として、通常のプリズムを用いることができる。
　撮像器４は、例えば、右眼カメラ５及び左眼カメラ６の双方を有するステレオカメラによって実現される。
　撮像器４は、移動体１に搭載されている。
　撮像器４は、測量対象物を複数回撮像し、複数の撮像画像を生成する。
　撮像器４は、それぞれの撮像画像を示す画像情報を記憶装置７に出力する。
　撮像器４は、任意の領域（以下「撮像対象領域」という）を撮像するものである。撮像対象領域は、例えば、測量装置２００による測量対象物を含む領域である。測量対象物は、例えば、地面又は構造物である。以下、測量対象物を含む領域が撮像対象領域に設定されており、かつ、測量対象物が地面である場合の例を中心に説明する。
　右眼カメラ５及び左眼カメラ６のそれぞれは、デジタルカメラによって実現される。
　右眼カメラ５の視野は、左眼カメラ６の視野と重複している。
　以下、右眼カメラ５により撮像された画像を「右眼画像」という。また、左眼カメラ６により撮像された画像を「左眼画像」という。また、右眼画像及び左眼画像を総称して「デジタル画像」ということがある。
　記憶装置７は、例えば、ＨＤＤ（Ｈａｒｄ　Ｄｉｓｋ　Ｄｒｉｖｅ）、又は、ＳＳＤ（Ｓｏｌｉｄ　Ｓｔａｔｅ　Ｄｒｉｖｅ）によって実現される。図１では、記憶装置７の記載が省略されている。
　記憶装置７は、測定器２から出力された位置情報及び撮像器４から出力された画像情報の双方を記憶するものである。
　演算装置８は、位置姿勢算出装置１００及び三次元形状算出部１５を備えている。図１では、演算装置８の記載が省略されている。
　位置姿勢算出装置１００は、位置情報取得部１１、画像情報取得部１２、情報抽出部１３及び位置姿勢算出部１４を備えている。
　位置姿勢算出装置１００は、撮像器４と反射材３とを備えている移動体１の位置及び移動体１の姿勢のそれぞれを算出する装置である。
　位置情報取得部１１は、例えば、図４に示す位置情報取得回路２１によって実現される。
　位置情報取得部１１は、記憶装置７から、測定器２による複数の測定結果のそれぞれを示す位置情報を取得する。
　位置情報取得部１１は、取得したそれぞれの位置情報を情報抽出部１３に出力する。
　画像情報取得部１２は、例えば、図４に示す画像情報取得回路２２によって実現される。
　画像情報取得部１２は、記憶装置７から、撮像器４によるそれぞれの撮像画像を示す画像情報を取得する。
　画像情報取得部１２は、取得したそれぞれの画像情報を情報抽出部１３に出力する。
　情報抽出部１３は、例えば、図４に示す情報抽出回路２３によって実現される。
　情報抽出部１３は、位置情報取得部１１により取得された複数の位置情報と画像情報取得部１２により取得された複数の画像情報との中から、測定器２による位置の測定タイミングと撮像器４による測量対象物の撮像タイミングとが同期している、位置情報と画像情報との組である情報対を抽出する。
　情報抽出部１３は、抽出した情報対を位置姿勢算出部１４に出力し、情報対に含まれている画像情報を三次元形状算出部１５に出力する。
　位置姿勢算出部１４は、例えば、図４に示す位置姿勢算出回路２４によって実現される。
　位置姿勢算出部１４は、情報抽出部１３により抽出された情報対から、移動体１の位置及び移動体１の姿勢のそれぞれを算出する。
　位置姿勢算出部１４は、移動体１の位置を示す位置情報及び移動体１の姿勢を示す姿勢情報のそれぞれを三次元形状算出部１５に出力する。
　図３に示す位置姿勢算出装置１００では、位置姿勢算出部１４により算出される移動体１の位置は、撮像器４の位置に相当し、位置姿勢算出部１４により算出される移動体１の姿勢は、撮像器４の姿勢に相当する。
　三次元形状算出部１５は、例えば、図４に示す三次元形状算出回路２５によって実現される。
　三次元形状算出部１５は、情報抽出部１３により抽出された情報対に含まれている画像情報を取得する。
　三次元形状算出部１５は、位置姿勢算出部１４から、移動体１の位置を示す位置情報及び移動体１の姿勢を示す姿勢情報のそれぞれを取得する。
　三次元形状算出部１５は、画像情報が示す撮像画像と、位置情報が示す移動体１の位置と、姿勢情報が示す移動体１の姿勢とを用いて、測量対象物の三次元形状を算出する。
　三次元形状算出部１５は、例えば、いわゆる「多視点ステレオ」の技術を用いることによって、測量対象物の三次元形状を算出する。「多視点ステレオ」の技術自体は、公知の技術であるため詳細な説明を省略する。
　図３では、演算装置８の構成要素である位置情報取得部１１、画像情報取得部１２、情報抽出部１３、位置姿勢算出部１４及び三次元形状算出部１５のそれぞれが、図４に示すような専用のハードウェアによって実現されるものを想定している。即ち、演算装置８が、位置情報取得回路２１、画像情報取得回路２２、情報抽出回路２３、位置姿勢算出回路２４及び三次元形状算出回路２５によって実現されるものを想定している。
　位置情報取得回路２１、画像情報取得回路２２、情報抽出回路２３、位置姿勢算出回路２４及び三次元形状算出回路２５のそれぞれは、例えば、単一回路、複合回路、プログラム化したプロセッサ、並列プログラム化したプロセッサ、ＡＳＩＣ（Ａｐｐｌｉｃａｔｉｏｎ　Ｓｐｅｃｉｆｉｃ　Ｉｎｔｅｇｒａｔｅｄ　Ｃｉｒｃｕｉｔ）、ＦＰＧＡ（Ｆｉｅｌｄ－Ｐｒｏｇｒａｍｍａｂｌｅ　Ｇａｔｅ　Ａｒｒａｙ）、又は、これらを組み合わせたものが該当する。
　演算装置８の構成要素は、専用のハードウェアによって実現されるものに限るものではなく、演算装置８が、ソフトウェア、ファームウェア、又は、ソフトウェアとファームウェアとの組み合わせによって実現されるものであってもよい。
　ソフトウェア又はファームウェアは、プログラムとして、コンピュータのメモリに格納される。コンピュータは、プログラムを実行するハードウェアを意味し、例えば、ＣＰＵ（Ｃｅｎｔｒａｌ　Ｐｒｏｃｅｓｓｉｎｇ　Ｕｎｉｔ）、中央処理装置、処理装置、演算装置、マイクロプロセッサ、マイクロコンピュータ、プロセッサ、あるいは、ＤＳＰ（Ｄｉｇｉｔａｌ　Ｓｉｇｎａｌ　Ｐｒｏｃｅｓｓｏｒ）が該当する。
　図５は、演算装置８が、ソフトウェア又はファームウェア等によって実現される場合のコンピュータのハードウェア構成図である。
　演算装置８が、ソフトウェア又はファームウェア等によって実現される場合、位置情報取得部１１、画像情報取得部１２、情報抽出部１３、位置姿勢算出部１４及び三次元形状算出部１５におけるそれぞれの処理手順をコンピュータに実行させるためのプログラムがメモリ３１に格納される。そして、コンピュータのプロセッサ３２がメモリ３１に格納されているプログラムを実行する。
　また、図４では、演算装置８の構成要素のそれぞれが専用のハードウェアによって実現される例を示し、図５では、演算装置８がソフトウェア又はファームウェア等によって実現される例を示している。しかし、これは一例に過ぎず、演算装置８における一部の構成要素が専用のハードウェアによって実現され、残りの構成要素がソフトウェア又はファームウェア等によって実現されるものであってもよい。
　次に、位置姿勢算出装置１００が、移動体１の位置及び移動体１の姿勢のそれぞれを算出する際に使用する座標系について説明する。
　位置姿勢算出装置１００が使用する座標系として、世界座標系、右眼カメラ座標系、左眼カメラ座標系、移動体座標系、デジタル画像座標系及び正規化画像座標系がある。
　世界座標系は、図１に示すように、互いに直交するＸ軸、Ｙ軸及びＺ軸を有する右手系の三次元座標系である。
　世界座標系におけるＸＹ平面は、測量対象物である地面に対して平行、又は、地面に対して略平行である。世界座標系におけるＺ軸の正方向は、天頂方向を示している。図１に示す測量装置２００では、移動体１の進行方向が、例えば、世界座標系におけるＸ軸の正方向であるものとする。
　図６は、右眼カメラ座標系、正規化画像座標系、デジタル画像座標系及び世界座標系を示す説明図である。
　右眼カメラ座標系は、図６に示すように、互いに直交するＸ軸、Ｙ軸及びＺ軸を有する右手系の三次元座標系である。
　右眼カメラ座標系の原点は、右眼カメラ５の投影中心に対応している。右眼カメラ座標系におけるＸＹ平面は、右眼カメラ５の投影面に対して平行である。
　右眼カメラ座標系におけるＸ軸の正方向は、右眼カメラ５の投影面における上方向に対応している。右眼カメラ座標系におけるＹ軸の正方向は、右眼カメラ５の投影面における左方向に対応している。
　左眼カメラ座標系（図６には記載していない）は、右眼カメラ座標系と同様に、互いに直交するＸ軸、Ｙ軸及びＺ軸を有する右手系の三次元座標系である。
　左眼カメラ座標系の原点は、左眼カメラ６の投影中心に対応している。左眼カメラ座標系におけるＸＹ平面は、左眼カメラ６の投影面に対して平行である。
　左眼カメラ座標系におけるＸ軸の正方向は、左眼カメラ６の投影面における上方向に対応している。左眼カメラ座標系におけるＹ軸の正方向は、左眼カメラ６の投影面における左方向に対応している。
　移動体座標系は、互いに直交するＸ軸、Ｙ軸及びＺ軸を有する任意の三次元座標系である。
　移動体座標系は、移動体１を基準とする座標系である。移動体座標系は、測量装置２００の用途等に応じて設定されるものである。具体的には、例えば、移動体座標系は、図６に示す右眼カメラ座標系、又は、左眼カメラ座標系と一致する座標系に設定される。
　図１に示す測量装置２００では、移動体座標系と右眼カメラ座標系とが一致しているものとする。したがって、位置姿勢算出部１４により算出される移動体１の位置は、右眼カメラ５の位置となり、位置姿勢算出部１４により算出される移動体１の姿勢は、右眼カメラ５の姿勢となる。
　図１に示す測量装置２００では、右眼カメラ５の投影面における上方向が、移動体１の正面方向と概ね一致しているものとする。
　移動体座標系の原点は、移動体１の位置に対応している。したがって、世界座標系における移動体座標系の原点の位置を示す座標は、世界座標系における移動体１の位置を示すものである。
　以下、世界座標系、右眼カメラ座標系、左眼カメラ座標系又は移動体座標系におけるそれぞれの位置座標を「三次元座標」という。特に、世界座標系における複数の後述するタイポイントにおけるそれぞれの三次元座標を「タイポイント位置」ということがある。また、右眼カメラ５の投影面に沿う二次元座標系、又は、左眼カメラ６の投影面に沿う二次元座標系における位置座標を「投影座標」という。
　デジタル画像座標系は、図６に示すように、互いに直交するＵ軸及びＶ軸を有する画像座標系である。
　デジタル画像座標系における原点は、それぞれのデジタル画像における左上の隅に対応している。デジタル画像座標系におけるＵ軸の正方向は、それぞれのデジタル画像における右方向に対応している。デジタル画像座標系におけるＶ軸の正方向は、それぞれのデジタル画像における下方向に対応している。
　正規化画像座標系は、図６に示すように、互いに直交するＵ軸及びＶ軸を有する画像座標系である。
　正規化画像座標系における原点は、それぞれのデジタル画像における中心に対応している。正規化画像座標系におけるＵ軸の正方向は、それぞれのデジタル画像における上方向に対応している。正規化画像座標系におけるＶ軸の正方向は、それぞれのデジタル画像における左方向に対応している。
　以下、デジタル画像座標系又は正規化画像座標系におけるそれぞれの位置座標を「画像座標」という。特に、デジタル画像座標系又は正規化画像座標系におけるそれぞれのタイポイントの画像座標を「タイポイント座標」ということがある。
　次に、測量装置２００の動作について説明する。
　図７は、実施の形態１に係る位置姿勢算出装置１００を含む演算装置８の処理手順を示すフローチャートである。
　図８は、位置姿勢算出装置１００における位置姿勢算出部１４の処理手順を示すフローチャートである。
　まず、移動体１は、世界座標系におけるＸＹ平面に沿って移動する。
　図１に示す測量装置２００では、移動体１の移動方向が、例えば、世界座標系におけるＸ軸の正方向、かつ、Ｙ軸及びＺ軸のそれぞれと直交する方向である。
　移動体１は、例えば、一定の時間間隔毎に静止し、移動と静止が交互に繰り返される。ただし、移動体１が静止する時間間隔は、測定器２による反射材３の位置の測定間隔及び撮像器４による測定対象物の撮像間隔のそれぞれと異なっていてもよい。
　測定器２は、移動体１に設けられている反射材３の位置を複数回測定し、複数の測定結果のそれぞれを示す位置情報を記憶装置７に出力する。
　即ち、測定器２は、移動体１が移動と静止を繰り返している期間中、反射材３を追尾しながら、一定の時間間隔で、反射材３に向けて位置測定用の光を照射する。
　測定器２は、反射材３によって反射された光を受信する。
　測定器２は、例えば、光を照射してから光を受信するまでに要した時間と、光の照射方向とから、反射材３の位置を算出する。
　測定器２により測定された位置は、世界座標系における反射材３の位置の三次元座標（ｘｐｗ，ｙｐｗ，ｚｐｗ）である。
　撮像器４は、測量対象物を複数回撮像して、複数の撮像画像を生成し、それぞれの撮像画像を示す画像情報を記憶装置７に出力する。
　即ち、撮像器４の右眼カメラ５は、移動体１が移動と静止を繰り返している期間中、測量対象物を含む撮像対象領域を複数回撮像する。
　右眼カメラ５は、撮像したそれぞれの右眼画像であるデジタル画像を示す画像情報を記憶装置７に出力する。
　撮像器４の左眼カメラ６は、移動体１が移動と静止を繰り返している期間中、測量対象物を含む撮像対象領域を複数回撮像する。
　左眼カメラ６は、撮像したそれぞれの左眼画像であるデジタル画像を示す画像情報を記憶装置７に出力する。
　図９は、測定器２による反射材３の位置の測定間隔及び撮像器４による測定対象物の撮像間隔を示す説明図である。
　図９において、黒で塗り潰された〇は、カメラ撮影タイミングを示しており、カメラ撮影タイミングは、右眼カメラ５及び左眼カメラ６が、撮像対象領域を撮像したタイミングである。
　斜線が施されている〇は、ＴＳ計測タイミングを示しており、ＴＳ計測タイミングは、測定器２が反射材３の位置を測定したタイミングである。
　また、黒で塗り潰された〇及び斜線が施されている〇のそれぞれを囲んでいる楕円は、移動体１が静止しているときの、カメラ撮影タイミング及びＴＳ計測タイミングのそれぞれを示している。
　楕円に囲まれていない黒で塗り潰された〇は、移動体１が移動しているときのカメラ撮影タイミングであり、楕円に囲まれていない斜線が施されている〇は、移動体１が移動しているときのＴＳ計測タイミングである。
　位置姿勢算出装置１００の位置情報取得部１１は、記憶装置７から、測定器２によるそれぞれの測定結果を示す位置情報を取得する（図７のステップＳＴ１）。
　位置情報取得部１１は、取得したそれぞれの位置情報を情報抽出部１３に出力する。
　位置姿勢算出装置１００の画像情報取得部１２は、記憶装置７から、撮像器４により生成されたそれぞれの撮像画像を示す画像情報を取得する（図７のステップＳＴ２）。
　画像情報取得部１２は、取得したそれぞれの画像情報を情報抽出部１３に出力する。
　位置姿勢算出装置１００の情報抽出部１３は、位置情報取得部１１から、複数の位置情報を取得し、画像情報取得部１２から、複数の画像情報を取得する。
　情報抽出部１３は、複数の位置情報と複数の画像情報との中から、測定器２による位置の測定タイミングと撮像器４による測量対象物の撮像タイミングとが同期している、位置情報と画像情報との組である情報対を抽出する（図７のステップＳＴ３）。
　情報抽出部１３は、抽出した情報対を位置姿勢算出部１４に出力する。
　以下、情報抽出部１３による位置情報と画像情報との組の抽出処理を具体的に説明する。
　撮像器４によるカメラ撮影タイミングは、図９に示すように、一定間隔である。
　情報抽出部１３は、一定間隔で撮像された複数の撮像画像を互いに比較し、撮像画像の比較結果に基づいて、複数の撮像画像間の移動量を計算する。複数の撮像画像間の移動量は、例えば、テンプレートマッチング又はオプティカルフロー推定の方法を用いることで求めることができる。
　情報抽出部１３は、複数の撮像画像間の移動量に基づいて、それぞれのカメラ撮影タイミングの間で、移動体１が静止しているのか、移動しているのかを判定する。
　即ち、情報抽出部１３は、複数の撮像画像間の移動量と、移動判定用の閾値Ｔｈ１とを比較し、移動量が移動判定用の閾値Ｔｈ１以下であれば、移動体１が静止していると判定する。情報抽出部１３は、移動量が移動判定用の閾値Ｔｈ１よりも大きければ、移動体１が移動していると判定する。移動判定用の閾値Ｔｈ１は、情報抽出部１３の内部メモリに格納されていてもよいし、位置姿勢算出装置１００の外部から与えられるものであってもよい。
　図１０は、移動体１が静止しているときのカメラ撮影タイミング及び移動体１が移動しているときのカメラ撮影タイミングを示す説明図である。
　楕円で囲まれているカメラ撮影タイミングは、移動体１が静止しているときのカメラ撮影タイミングである。即ち、移動体１が僅かに移動していても、移動速度が低いため、移動体１が静止しているものとして扱うときのカメラ撮影タイミングである。楕円で囲まれていないカメラ撮影タイミングは、移動体１が移動しているときのカメラ撮影タイミングである。
　図１０の例では、連続している９つのカメラ撮影タイミングが、移動体１が静止しているときのカメラ撮影タイミングとして判定されている。
　情報抽出部１３は、図１０に示すように、移動体１が静止しているときのカメラ撮影タイミングが連続していれば、連続しているカメラ撮影タイミングの中から、撮像時刻が中間のカメラ撮影タイミングを特定する。
　即ち、情報抽出部１３は、連続しているカメラ撮影タイミングの中で、撮像時刻が最も古いカメラ撮影タイミングＴ１と、撮像時刻が最も新しいカメラ撮影タイミングＴ２とから、以下の式（１）に示すように、中間の撮像時刻Ｔｍｉｄ１を算出する。
Ｔｍｉｄ１＝（Ｔ１＋Ｔ２）／２　　　　　　　　　　（１）
　情報抽出部１３は、連続しているカメラ撮影タイミングの撮像時刻の中で、中間の撮像時刻Ｔｍｉｄ１に最も近い撮像時刻を特定し、特定した撮像時刻に係るカメラ撮影タイミングを、撮像時刻が中間のカメラ撮影タイミングであると特定する。
　情報抽出部１３は、撮像時刻が中間のカメラ撮影タイミングに係る撮像画像を、静止中の撮像画像であるとする。情報抽出部１３は、連続しているカメラ撮影タイミングの中で、撮像時刻が中間ではないカメラ撮影タイミングに係る撮像画像を破棄する。
　測定器２によるＴＳ計測タイミングは、図９に示すように、一定間隔である。
　情報抽出部１３は、一定間隔で算出された複数の位置を互いに比較し、位置の比較結果に基づいて、複数の位置間の移動量を計算する。複数の位置間の移動量は、例えば、ユークリッド距離として求めることができる。
　情報抽出部１３は、複数の位置間の移動量に基づいて、それぞれのＴＳ計測タイミングの間で、移動体１が静止しているのか、移動しているのかを判定する。
　即ち、情報抽出部１３は、複数の位置間の移動量と、移動判定用の閾値Ｔｈ２とを比較し、移動量が移動判定用の閾値Ｔｈ２以下であれば、移動体１が静止していると判定する。情報抽出部１３は、移動量が移動判定用の閾値Ｔｈ２よりも大きければ、移動体１が移動していると判定する。移動判定用の閾値Ｔｈ２は、情報抽出部１３の内部メモリに格納されていてもよいし、位置姿勢算出装置１００の外部から与えられるものであってもよい。
　情報抽出部１３は、移動体１が静止しているときのＴＳ計測タイミングが連続していれば、連続しているＴＳ計測タイミングの中から、位置算出時刻が中間のＴＳ計測タイミングを特定する。
　即ち、情報抽出部１３は、連続しているＴＳ計測タイミングの中で、位置算出時刻が最も古いＴＳ計測タイミングＴ３と、位置算出時刻が最も新しいＴＳ計測タイミングＴ４とから、以下の式（２）に示すように、中間の位置算出時刻Ｔｍｉｄ２を算出する。
Ｔｍｉｄ２＝（Ｔ３＋Ｔ４）／２　　　　　　　　　　（２）
　情報抽出部１３は、連続しているＴＳ計測タイミングの位置算出時刻の中で、中間の位置算出時刻Ｔｍｉｄ２に最も近い位置算出時刻を特定し、特定した位置算出時刻に係るＴＳ計測タイミングを、位置算出時刻が中間のＴＳ計測タイミングであると特定する。
　情報抽出部１３は、位置算出時刻が中間のＴＳ計測タイミングに係る位置を、静止中の位置であるとする。情報抽出部１３は、連続しているＴＳ計測タイミングの中で、位置算出時刻が中間ではないＴＳ計測タイミングに係る位置を破棄する。
　情報抽出部１３は、破棄せずに残っている複数の位置のそれぞれを示す位置情報と、破棄せずに残っている複数の撮像画像のそれぞれを示す画像情報との中から、位置の測定タイミングと測量対象物の撮像タイミングとが同期している、位置情報と画像情報との組を抽出する。
　破棄せずに残っている位置は、破棄せずに残っている、静止しているときの位置と、移動しているときの位置とを含んでいる。また、破棄せずに残っている撮像画像は、破棄せずに残っている、静止しているときの撮像画像と、移動しているときの撮像画像とを含んでいる。
　図９の例では、図中、左側から１番目の撮像画像のカメラ撮影タイミングは、破棄せずに残っている複数の位置のＴＳ計測タイミングの中で、左側から１番目の位置のＴＳ計測タイミングと最も近い。図２に示す測量装置では、カメラ撮影タイミングと最も近いＴＳ計測タイミングは、カメラ撮影タイミングと完全に一致している場合だけでなく、カメラ撮影タイミングとずれていても同期しているものとして扱う。この場合、カメラ撮影タイミングとＴＳ計測タイミングとのずれは、最大でも、カメラ撮影タイミングの間隔の半分であり、カメラ撮影タイミングとＴＳ計測タイミングとのずれが、カメラ撮影タイミングの間隔以上の大きなものになることがない。
　ここでの測定タイミングと撮像タイミングとの同期は、測定タイミングの時刻とカメラ撮像タイミングの時刻とが完全に一致しているものに限るものではなく、実用上問題のない範囲で、測定タイミングの時刻とカメラ撮像タイミングの時刻とがずれているものを含んでいる。
　情報抽出部１３は、左側から１番目の撮像画像のカメラ撮影タイミングと、左側から１番目の位置のＴＳ計測タイミングとが同期しているものとして、左側から１番目の位置を示す位置情報と左側から１番目の撮像画像を示す画像情報との組を情報対とする。
　また、図中、左側から２番目の撮像画像のカメラ撮影タイミングは、破棄せずに残っている複数の位置のＴＳ計測タイミングの中で、左側から２番目の位置のＴＳ計測タイミングと最も近い。
　情報抽出部１３は、左側から２番目の撮像画像のカメラ撮影タイミングと、左側から２番目の位置のＴＳ計測タイミングとが同期しているものとして、左側から２番目の位置を示す位置情報と左側から２番目の撮像画像を示す画像情報との組を情報対とする。
　また、図中、左側から３番目の撮像画像のカメラ撮影タイミングは、破棄せずに残っている複数の位置のＴＳ計測タイミングの中で、左側から２番目の位置のＴＳ計測タイミングと最も近い。
　情報抽出部１３は、左側から３番目の撮像画像のカメラ撮影タイミングと、左側から２番目の位置のＴＳ計測タイミングとが同期しているものとして、左側から２番目の位置を示す位置情報と左側から３番目の撮像画像を示す画像情報との組を情報対とする。
　また、図中、左側から４番目の撮像画像のカメラ撮影タイミングは、破棄せずに残っている複数の位置のＴＳ計測タイミングの中で、左側から３番目の位置のＴＳ計測タイミングと最も近い。
　情報抽出部１３は、左側から４番目の撮像画像のカメラ撮影タイミングと、左側から３番目の位置のＴＳ計測タイミングとが同期しているものとして、左側から３番目の位置を示す位置情報と左側から４番目の撮像画像を示す画像情報との組を情報対とする。
　また、図中、左側から５番目の撮像画像のカメラ撮影タイミングは、破棄せずに残っている複数の位置のＴＳ計測タイミングの中で、左側から４番目の位置のＴＳ計測タイミングと最も近い。
　情報抽出部１３は、左側から５番目の撮像画像のカメラ撮影タイミングと、左側から４番目の位置のＴＳ計測タイミングとが同期しているものとして、左側から４番目の位置を示す位置情報と左側から５番目の撮像画像を示す画像情報との組を情報対とする。
　位置姿勢算出部１４は、情報抽出部１３から、複数の情報対を取得する。
　位置姿勢算出部１４は、それぞれの情報対から、移動体１の位置及び移動体１の姿勢のそれぞれを算出する（図７のステップＳＴ４）。
　位置姿勢算出部１４は、移動体１の位置を示す位置情報及び移動体１の姿勢を示す姿勢情報のそれぞれを三次元形状算出部１５に出力する。
　以下、位置姿勢算出部１４による位置及び姿勢の算出処理を具体的に説明する。
　位置姿勢算出部１４では、撮像対象領域における複数の地点のそれぞれに対応するデジタル画像上の点をタイポイントとする。それぞれの地点の位置は、測量装置２００において、既知であってもよいし、未知であってもよい。それぞれの地点は、撮像対象領域に設けられているマーカの位置であってもよい。
　以下、それぞれのタイポイントが地点に対応するものである場合において、かかる地点の位置が未知であるとき、かかる地点を「未知の地点」ということがある。また、それぞれのタイポイントがマーカに対応するものである場合において、かかるマーカの位置が既知であるとき、かかるマーカを「既知のマーカ」ということがある。また、それぞれのタイポイントがマーカに対応するものである場合において、かかるマーカの位置が未知であるとき、かかるマーカを「未知のマーカ」ということがある。
　まず、位置姿勢算出部１４は、情報抽出部１３から、複数の情報対を取得する。
　位置姿勢算出部１４は、それぞれの情報対に含まれている画像情報が示す撮像画像であるデジタル画像におけるそれぞれのタイポイントの画像座標（ｕ，ｖ）を算出する（図８のステップＳＴ１１）。
　即ち、位置姿勢算出部１４は、それぞれのタイポイントが未知の地点に対応していれば、それぞれのデジタル画像に対する画像処理を実行することによって、複数のデジタル画像に共通して含まれている特徴点を抽出する。
　位置姿勢算出部１４は、抽出した特徴点がタイポイントであるとして、抽出した特徴点の画像座標（ｕ，ｖ）を算出する。
　特徴点の抽出処理自体は、公知の技術であるため詳細な説明を省略する。特徴点の抽出処理は、例えば、以下の参考文献１に記載されている。
［参考文献１］
David G. Lowe, "Distinctive Image Features from Scale-Invariant Keypoints," International Journal of Computer Vision, 2004, 60(2), pp. 91-110.
　位置姿勢算出部１４は、それぞれのタイポイントが、既知のマーカ又は未知のマーカに対応していれば、それぞれのデジタル画像に対するテンプレートマッチングを実行することによって、それぞれのデジタル画像に含まれているマーカを検出する。
　位置姿勢算出部１４は、検出したマーカがタイポイントであるとして、検出したマーカの画像座標（ｕ，ｖ）を算出する。
　また、それぞれのタイポイントが、既知のマーカ又は未知のマーカに対応している場合、測量装置２００のユーザが、それぞれのデジタル画像に含まれているマーカの位置を目視によって確認し、ユーザが、確認した位置を位置姿勢算出部１４に与えるようにしてもよい。位置姿勢算出部１４は、ユーザによって与えられた位置に対応する画像座標（ｕ，ｖ）を算出する。
　次に、位置姿勢算出部１４は、世界座標系におけるそれぞれのタイポイントの三次元座標（ｘＴ，ｙＴ，ｚＴ）の初期値を算出する（図８のステップＳＴ１２）。
　以下、位置姿勢算出部１４による三次元座標（ｘＴ，ｙＴ，ｚＴ）の初期値の算出処理を具体的に説明する。
　位置姿勢算出部１４は、複数の情報対に含まれている位置情報が示す位置の全てのＸ座標（ｘｐｗ）の平均値を算出する。
　位置姿勢算出部１４は、Ｘ座標（ｘｐｗ）の平均値を、それぞれのタイポイントのＸ座標（ｘＴ）の初期値に用いる。
　位置姿勢算出部１４は、複数の情報対に含まれている位置情報が示す位置の全てのＹ座標（ｙｐｗ）の平均値を算出する。
　位置姿勢算出部１４は、Ｙ座標（ｙｐｗ）の平均値を、それぞれのタイポイントのＹ座標（ｙＴ）の初期値に用いる。
　位置姿勢算出部１４は、複数の情報対に含まれている位置情報が示す位置のＺ座標（ｚｐｗ）を用いて、地面における任意の地点のＺ座標を算出する。地面における任意の地点のＺ座標は、地面の高さに近い位置のＺ座標である。
　具体的には、例えば、位置姿勢算出部１４は、複数の情報対に含まれている位置情報が示す位置の全てのＺ座標（ｚｐｗ）の平均値を算出する。そして、位置姿勢算出部１４は、Ｚ座標（ｚｐｗ）の平均値から、移動体１に対して反射材３が設置されている位置の高さを示す既知のＺ座標を減算することによって、地面における任意の地点のＺ座標を算出する。
　位置姿勢算出部１４は、地面における任意の地点のＺ座標を、それぞれのタイポイントのＺ座標（ｚＴ）の初期値に用いる。
　次に、位置姿勢算出部１４は、右眼カメラ５の姿勢角度（ωｒｗ，φｒｗ，κｒｗ）の初期値を算出する（図８のステップＳＴ１３）。
　以下、位置姿勢算出部１４による姿勢角度（ωｒｗ，φｒｗ，κｒｗ）の初期値の算出処理を具体的に説明する。
　位置姿勢算出部１４では、時刻ｔにおける反射材３の三次元座標が（ｘ，ｙ，ｚ）であるものとする。時刻ｔに対応するＴＳ計測タイミングの次のＴＳ計測タイミングに対応する時刻がｔ’であるものとする。
　位置姿勢算出部１４では、時刻ｔ’における反射材３の三次元座標が（ｘ’，ｙ’，ｚ’）であるものとする。
　右眼カメラ座標系におけるＸ軸の正方向は、右眼カメラ５の投影面における上方向に対応しており、移動体１の正面方向を示すものとなる。図１に示す測量装置２００では、移動体１の進行方向が、移動体１の正面方向と同一又は略同一の方向に設定されている。
　したがって、右眼カメラ５の姿勢角度（ωｒｗ，φｒｗ，κｒｗ）は、Ｘ座標（１，０，０）を反射材３の移動ベクトル（ｘ－ｘ’，ｙ－ｙ’，ｚ－ｚ’）と一致させる回転角によって近似される。
　図１に示す測量装置２００では、右眼カメラ５が、世界座標系におけるＸＹ平面に沿って移動する。このため、ロール角（ωｒｗ）及びピッチ角（φｒｗ）のそれぞれは、０に近似される。また、ヨー角（κｒｗ）は、ＸＹ平面におけるベクトル（１，０）をベクトル（ｘ－ｘ’，ｙ－ｙ’）と一致させる回転角によって近似される。したがって、ヨー角（κｒｗ）について、以下の式（３）が成立する。

Figure JPOXMLDOC01-appb-I000001
　位置姿勢算出部１４は、式（３）によってヨー角（κｒｗ）を算出する。ヨー角（κｒｗ）は、逆三角関数により算出される。
　なお、時刻ｔに対応するＴＳ計測タイミングの１つ前のＴＳ計測タイミングに対応する時刻がｔ”であるものとする。時刻ｔよりも後の時刻における反射材３の位置が未測定である場合、時刻ｔにおける姿勢角度（ωｒｗ，φｒｗ，κｒｗ）の初期値に、時刻ｔ”における姿勢角度（ωｒｗ，φｒｗ，κｒｗ）の初期値が用いられる。時刻ｔ”における姿勢角度（ωｒｗ，φｒｗ，κｒｗ）の初期値は、時刻ｔにおける反射材３の三次元座標と、時刻ｔ”における反射材３の三次元座標とを用いて算出される。
　次に、位置姿勢算出部１４は、右眼カメラ５の位置の三次元座標（ｘｒｗ，ｙｒｗ，ｚｒｗ）の初期値を算出する（図８のステップＳＴ１４）。
　以下、位置姿勢算出部１４による三次元座標（ｘｒｗ，ｙｒｗ，ｚｒｗ）の初期値の算出処理を具体的に説明する。
　位置姿勢算出部１４では、世界座標系における反射材３の位置の三次元座標が（ｘｐｗ，ｙｐｗ，ｚｐｗ）であるものとする。また、右眼カメラ座標系における反射材３の位置の三次元座標が（ｘｐｒ，ｙｐｒ，ｚｐｒ）であるものとする。
　位置姿勢算出部１４では、世界座標系における右眼カメラ５の姿勢角度が（ωｒｗ，φｒｗ，κｒｗ）であるものとする。
　この場合、右眼カメラ５の位置の三次元座標（ｘｒｗ，ｙｒｗ，ｚｒｗ）は、以下の式（４）及び式（５）によって表される。

Figure JPOXMLDOC01-appb-I000002
　位置姿勢算出部１４は、ステップＳＴ１３で算出した右眼カメラ５の姿勢角度（ωｒｗ，φｒｗ，κｒｗ）の初期値を式（５）に代入し、式（４）に基づいて、右眼カメラ５の位置の三次元座標（ｘｒｗ，ｙｒｗ，ｚｒｗ）の初期値を算出する。
　次に、位置姿勢算出部１４は、タイポイントの三次元座標（ｘＴ，ｙＴ，ｚＴ）の初期値を実際の値に近づけ、右眼カメラ５の姿勢角度（ωｒｗ，φｒｗ，κｒｗ）の初期値を実際の値に近づけ、右眼カメラ５の位置の三次元座標（ｘｒｗ，ｙｒｗ，ｚｒｗ）の初期値を実際の値に近づける最適化処理を実施する（図８のステップＳＴ１５）。
　以下、位置姿勢算出部１４による最適化処理を具体的に説明する。
　まず、位置姿勢算出部１４は、以下に示すように、右眼カメラ５におけるそれぞれのタイポイントの投影座標（０ｕｒ，０ｖｒ）を算出する。
　また、位置姿勢算出部１４は、左眼カメラ６におけるそれぞれのタイポイントの投影座標（０ｕｌ，０ｖｌ）を算出する。
　これらの投影座標（０ｕｒ，０ｖｒ），（０ｕｌ，０ｖｌ）は、正規化画像座標系における位置座標である。
　世界座標系における反射材３の位置の三次元座標（ｘｐｗ，ｙｐｗ，ｚｐｗ）、右眼カメラ座標系における反射材３の位置の三次元座標（ｘｐｒ，ｙｐｒ，ｚｐｒ）及び世界座標系における右眼カメラ５の姿勢角度（ωｒｗ，φｒｗ，κｒｗ）が与えられるものとする。この場合、任意の点Ｘについて、世界座標系における三次元座標Ｘｗと、右眼カメラ座標系における三次元座標Ｘｒとの変換式は、式（４）及び以下の式（６）によって表される。

Figure JPOXMLDOC01-appb-I000003
　右眼カメラ５の焦点距離がｆｒであり、右眼カメラ５における撮像素子の幅がＣｒであり、右眼画像の横幅がＷｒであるものとする。このとき、三次元座標Ｘｗを投影座標（０ｕｒ，０ｖｒ）に変換する変換式は、以下の式（７）及び式（８）によって表される。式（７）において、Ｔは、転置を表す数学記号である。

Figure JPOXMLDOC01-appb-I000004
　位置姿勢算出部１４は、世界座標系における三次元座標Ｘｗに対応する三次元座標を有するタイポイントについて、式（７）及び式（８）に従って、右眼カメラ５における投影座標（０ｕｒ，０ｖｒ）を算出する。
　位置姿勢算出部１４は、左眼カメラ６におけるそれぞれのタイポイントの投影座標（０ｕｌ，０ｖｌ）についても、右眼カメラ５におけるそれぞれのタイポイントの投影座標（０ｕｒ，０ｖｒ）と同様に算出する。
　具体的には、世界座標系における左眼カメラ座標系の原点の三次元座標が（ｘｌｗ，ｙｌｗ，ｚｌｗ）であるものとする。また、世界座標系の座標軸に対する左眼カメラ座標系の座標軸の回転角が（ωｌｗ，φｌｗ，κｌｗ）であるものとする。また、左眼カメラ６の焦点距離がｆｌであり、左眼カメラ６における撮像素子の幅がＣｌであり、左眼画像の横幅がＷｌであるものとする。
　位置姿勢算出部１４は、これらの値に基づき、三次元座標Ｘｗを投影座標（０ｕｌ，０ｖｌ）に変換する変換式を用いて、左眼カメラ６における投影座標（０ｕｌ，０ｖｌ）を算出する。
　ここで、世界座標系における左眼カメラ座標系の原点の三次元座標（ｘｌｗ，ｙｌｗ，ｚｌｗ）及び世界座標系の座標軸に対する左眼カメラ座標系の座標軸の回転角（ωｌｗ，φｌｗ，κｌｗ）は、以下の式（９）～式（１２）によって表される。即ち、三次元座標（ｘｌｗ，ｙｌｗ，ｚｌｗ）及び回転角（ωｌｗ，φｌｗ，κｌｗ）は、世界座標系における右眼カメラ座標系の原点の三次元座標（ｘｒｗ，ｙｒｗ，ｚｒｗ）、世界座標系の座標軸に対する右眼カメラ座標系の座標軸の回転角（ωｒｗ，φｒｗ，κｒｗ）、右眼カメラ座標系における左眼カメラ座標系の原点の三次元座標（ｘｌｒ，ｙｌｒ，ｚｌｒ）及び右眼カメラ座標系の座標軸に対する左眼カメラ座標系の座標軸の回転角（ωｌｒ，φｌｒ，κｌｒ）によって表される。

Figure JPOXMLDOC01-appb-I000005
　次に、位置姿勢算出部１４は、以下に示すように、ステップＳＴ１１で算出したタイポイントの画像座標（ｕ，ｖ）における歪みを補正する。
　位置姿勢算出部１４では、右眼画像において、デジタル画像座標系におけるタイポイントの画像座標が（１ｕｒ，１ｖｒ）であるものとする。右眼カメラ５の主点位置を示す位置座標が（Ｘｐｒ，Ｙｐｒ）であり、右眼カメラ５における歪み係数がｋ１ｒ、ｋ２ｒ、ｋ３ｒ、ｐ１ｒ、ｐ２ｒ、ｂ１ｒ及びｂ２ｒであるものとする。また、右眼カメラ５における撮像素子の幅がＣｒであり、右眼画像の横幅がＷｒであり、右眼画像の縦幅がＨｒであるものとする。
　このとき、デジタル画像座標系における歪みを有するタイポイントの座標（１ｕｒ，１ｖｒ）を、正規化画像座標系における歪み補正後のタイポイントの座標（２ｕｒ，２ｖｒ）に変換する変換式は、以下の式（１３）～式（１９）によって表される。

Figure JPOXMLDOC01-appb-I000006

Figure JPOXMLDOC01-appb-I000007

Figure JPOXMLDOC01-appb-I000008
　位置姿勢算出部１４は、式（１３）～式（１９）に従って、デジタル画像座標系における歪みを有するタイポイントの座標（１ｕｒ，１ｖｒ）を、正規化画像座標系における歪み補正後のタイポイントの座標（２ｕｒ，２ｖｒ）に変換する。
　また、位置姿勢算出部１４は、上記の変換と同様に、デジタル画像座標系における歪みを有するタイポイントの座標（１ｕｌ，１ｖｌ）を、正規化画像座標系における歪み補正後のタイポイントの座標（２ｕｌ，２ｖｌ）に変換する。
　即ち、位置姿勢算出部１４では、左眼画像において、デジタル画像座標系におけるタイポイントの画像座標が（１ｕｌ，１ｖｌ）であるものとする。左眼カメラ６の主点位置を示す位置座標が（Ｘｐｌ，Ｙｐｌ）であり、左眼カメラ６における歪み係数がｋ１ｌ、ｋ２ｌ、ｋ３ｌ、ｐ１ｌ、ｐ２ｌ、ｂ１ｌ及びｂ２ｌであるものとする。また、左眼カメラ６における撮像素子の幅がＣｌであり、左眼カメラ６の横幅がＷｌであり、左眼画像の縦幅がＨｌであるものとする。このとき、位置姿勢算出部１４は、式（１３）～式（１９）と同様の変換式を用いて、デジタル画像座標系における歪みを有するタイポイントの座標（１ｕｌ，１ｖｌ）を、正規化画像座標系における歪み補正後のタイポイントの座標（２ｕｌ，２ｖｌ）に変換する。
　次に、位置姿勢算出部１４は、算出した右眼カメラ５における投影座標（０ｕｒ，０ｖｒ）と、算出したタイポイントの座標（２ｕｒ，２ｖｒ）との差分値を最小化する。
　また、位置姿勢算出部１４は、算出した左眼カメラ６における投影座標（０ｕｌ，０ｖｌ）と、算出したタイポイントの座標（２ｕｌ，２ｖｌ）との差分値を最小化する。
　最適化対象のパラメータは、世界座標系におけるそれぞれのタイポイントの三次元座標（ｘＴ，ｙＴ，ｚＴ）、右眼カメラ５の姿勢角度（ωｒｗ，φｒｗ，κｒｗ）及び右眼カメラ５の位置の三次元座標（ｘｒｗ，ｙｒｗ，ｚｒｗ）である。最適化対象のパラメータ以外のパラメータは、いずれも定数とする。または、最適化対象のパラメータ以外のパラメータは、最適化対象のパラメータを用いて算出する。
　右眼カメラ５の位置の三次元座標（ｘｒｗ，ｙｒｗ，ｚｒｗ）は、上述したように、最適化対象のパラメータに含められているが、移動体１が静止しているときの右眼カメラ５の位置の三次元座標（ｘｒｗ，ｙｒｗ，ｚｒｗ）は、最適化対象のパラメータに含められない。即ち、位置姿勢算出部１４は、移動体１が静止しているときの右眼カメラ５の位置の三次元座標（ｘｒｗ，ｙｒｗ，ｚｒｗ）を最適化処理によって求めない。位置姿勢算出部１４は、式（４）に基づいて、世界座標系における反射材３の位置の三次元座標（ｘｐｗ，ｙｐｗ，ｚｐｗ）、右眼カメラ座標系における反射材３の位置の三次元座標（ｘｐｒ，ｙｐｒ，ｚｐｒ）及び世界座標系における右眼カメラ５の姿勢角度（ωｒｗ，φｒｗ，κｒｗ）を用いて、三次元座標（ｘｒｗ，ｙｒｗ，ｚｒｗ）を算出する。
　一部のタイポイントが既知のマーカに対応するものである場合、一部のタイポイントの三次元座標も定数とすることができる。これにより、残余のタイポイントの三次元座標の算出精度の向上を図ることができる。ただし、上記のとおり、全てのタイポイントが未知の地点又は未知のマーカに対応するものであってもよい。
  ステップＳＴ１５における最適化処理は、画像座標値の差分値の二乗和を最小化する非線形最小二乗問題であり、求解には、公知の種々の技術を用いることができる。これらの技術についての詳細な説明は省略する。具体的には、例えば、以下の参考文献２に記載された技術を用いることができる。
［参考文献2］
Levenberg, Kenneth. "A method for the solution of certain non-linear problems in least squares." Quarterly of applied mathematics 2.2 (1944): 164-168.
　上記のように、ステップＳＴ１～ＳＴ４の処理が実行されることにより、右眼カメラ５の位置と右眼カメラ５の姿勢とを高精度に算出することができる。
　通常の位置姿勢についての推定処理では、カメラの位置及び姿勢のそれぞれを未知数とする６自由度の最適化が行われる。これに対して、図１に示す測量装置２００では、移動体１が静止しているときの右眼カメラ５の姿勢のみを未知数とする３自由度の最適化を行うことにより、右眼カメラ５の位置と右眼カメラ５の姿勢とを高精度に算出することができる。
　三次元形状算出部１５は、情報抽出部１３により抽出された情報対に含まれている画像情報を取得する。
　三次元形状算出部１５は、位置姿勢算出部１４から、移動体１の位置を示す位置情報及び移動体１の姿勢を示す姿勢情報のそれぞれを取得する。移動体１の位置は、上述したように、右眼カメラ５の位置に相当し、移動体１の姿勢は、右眼カメラ５の姿勢に相当する。右眼カメラ５の位置は、三次元座標（ｘｒｗ，ｙｒｗ，ｚｒｗ）であり、右眼カメラ５の姿勢は、姿勢角度（ωｒｗ，φｒｗ，κｒｗ）である。
　三次元形状算出部１５は、画像情報が示す撮像画像と、位置情報が示す移動体１の位置と、姿勢情報が示す移動体１の姿勢とを用いて、測量対象物の三次元形状を算出する（図７のステップＳＴ５）。
　三次元形状算出部１５は、例えば、いわゆる「多視点ステレオ」の技術を用いることによって、測量対象物の三次元形状を算出する。
　右眼カメラ５の位置及び右眼カメラ５の姿勢のそれぞれが、位置姿勢算出部１４によって高精度に求められているため、三次元形状算出部１５による三次元形状の算出精度が向上する。
　図１に示す測量装置２００では、姿勢検出用のセンサを用いることなく、測量対象物の三次元形状を算出することができる。
　以上の実施の形態１では、測量対象物を複数回撮像し、複数の撮像画像を生成する撮像器４と、反射材３とを備えている移動体１の位置及び移動体１の姿勢のそれぞれを算出する位置姿勢算出装置１００であって、反射材３の位置を複数回測定する測定器２による複数の測定結果のそれぞれを示す位置情報を取得する位置情報取得部１１と、撮像器４により生成されたそれぞれの撮像画像を示す画像情報を取得する画像情報取得部１２と、位置情報取得部１１により取得された複数の位置情報と画像情報取得部１２により取得された複数の画像情報との中から、測定器２による位置の測定タイミングと撮像器４による測量対象物の撮像タイミングとが同期している、位置情報と画像情報との組である情報対を抽出する情報抽出部１３と、情報抽出部１３により抽出された情報対から、移動体１の位置及び移動体１の姿勢のそれぞれを算出する位置姿勢算出部１４とを備えるように、位置姿勢算出装置１００を構成した。したがって、位置姿勢算出装置１００は、姿勢検出器を実装することなく、移動体１の姿勢を算出することができる。
　図１に示す測量装置２００では、撮像器４が、右眼カメラ５及び左眼カメラ６の双方を有するステレオカメラによって実現されている。しかし、撮像器４は、１つ以上のデジタルカメラを実装しているものであればよい。
　撮像器４が実装しているデジタルカメラの個数を増やすことにより、移動体１の位置及び姿勢におけるそれぞれの推定精度を更に向上させることができる。撮像器４が実装しているデジタルカメラの個数を減らすことにより、測量装置２００を安価に実現することができる。
　撮像対象領域は、測量対象物を含む領域に限定されるものではない。また、測量対象物は、地面に限定されるものではない。例えば、測量対象物は、橋梁の表面又はトンネルの内面であってもよい。即ち、測量対象物は、水平面を含むものであってもよく、鉛直面を含むものであってもよく、傾斜面を含むものであってもよい。また、測量対象物は、平面状であってもよく、曲面状であってもよく、凹凸面状であってもよい。
　なお、測量対象物を含む領域と異なる領域が撮像対象領域に設定されている場合、移動体１に、距離測定用の測定器として、例えば、レーザスキャナが設けられているものであってもよい。
　三次元形状算出部１５は、距離測定用の測定器により測定された距離と、位置姿勢算出部１４によりそれぞれ算出された位置及び姿勢とを用いて、測量対象物の三次元形状を算出するものであってもよい。
　なお、本開示は、実施の形態の任意の構成要素の変形、もしくは実施の形態の任意の構成要素の省略が可能である。
　本開示に係る位置姿勢算出装置は、位置情報取得部、画像情報取得部、情報抽出部、及び位置姿勢算出部を備え、姿勢検出器を実装することなく、移動体の姿勢を算出することができ、移動体の姿勢の算出に適している。
　１　移動体、２　測定器、３　反射材、４　撮像器、５　右眼カメラ、６　左眼カメラ、７　記憶装置、８　演算装置、１１　位置情報取得部、１２　画像情報取得部、１３　情報抽出部、１４　位置姿勢算出部、１５　三次元形状算出部、２１　位置情報取得回路、２２　画像情報取得回路、２３　情報抽出回路、２４　位置姿勢算出回路、２５　三次元形状算出回路、３１　メモリ、３２　プロセッサ、１００　位置姿勢算出装置、２００　測量装置。

Claims (10)

  1. 　測量対象物を複数回撮像し、複数の撮像画像を生成する撮像器と、反射材とを備えている移動体の位置及び前記移動体の姿勢のそれぞれを算出する位置姿勢算出装置であって、
    　前記反射材の位置を複数回測定する測定器による複数の測定結果のそれぞれを示す位置情報を取得する位置情報取得部と、
    　前記撮像器により生成されたそれぞれの撮像画像を示す画像情報を取得する画像情報取得部と、
    　前記位置情報取得部により取得された複数の位置情報と前記画像情報取得部により取得された複数の画像情報との中から、前記測定器による位置の測定タイミングと前記撮像器による測量対象物の撮像タイミングとが同期している、位置情報と画像情報との組である情報対を抽出する情報抽出部と、
    　前記情報抽出部により抽出された情報対から、前記移動体の位置及び前記移動体の姿勢のそれぞれを算出する位置姿勢算出部と
    　を備えた位置姿勢算出装置。
  2. 　前記位置姿勢算出部は、前記情報抽出部により抽出された情報対から、前記移動体の位置の初期値及び前記移動体の姿勢の初期値のそれぞれを算出し、前記位置の初期値を前記位置の実際の値に近づけ、前記姿勢の初期値を前記姿勢の実際の値に近づける最適化処理として、前記移動体の姿勢のみを未知数とする３自由度の最適化処理を実施することを特徴とする請求項１記載の位置姿勢算出装置。
  3. 　前記情報抽出部は、前記位置情報取得部により取得された複数の位置情報の中から、前記移動体が静止しているときの測定結果を示す位置情報を抽出し、前記画像情報取得部により取得された複数の画像情報の中から、前記移動体が静止しているときの撮像画像を示す画像情報を抽出し、抽出した位置情報と抽出した画像情報との組を情報対として前記位置姿勢算出部に出力することを特徴とする請求項１記載の位置姿勢算出装置。
  4. 　前記情報抽出部は、前記画像情報取得部により取得された複数の画像情報が示す撮像画像を互いに比較し、撮像画像の比較結果に基づいて、前記複数の撮像画像の中で、前記移動体が静止しているときの撮像画像を特定することを特徴とする請求項３記載の位置姿勢算出装置。
  5. 　前記情報抽出部は、前記位置情報取得部により取得された複数の位置情報が示す位置を互いに比較し、位置の比較結果に基づいて、前記複数の測定結果の中で、前記移動体が静止しているときの測定結果を特定することを特徴とする請求項３記載の位置姿勢算出装置。
  6. 　測量対象物を複数回撮像し、複数の撮像画像を生成する撮像器と、反射材とを備えている移動体の位置及び前記移動体の姿勢のそれぞれを算出する位置姿勢算出方法であって、
    　位置情報取得部が、前記反射材の位置を複数回測定する測定器による複数の測定結果のそれぞれを示す位置情報を取得し、
    　画像情報取得部が、前記撮像器により生成された複数の撮像画像のそれぞれを示す画像情報を取得し、
    　情報抽出部が、前記位置情報取得部により取得された複数の位置情報と前記画像情報取得部により取得された複数の画像情報との中から、前記測定器による位置の測定タイミングと前記撮像器による測量対象物の撮像タイミングとが同期している、位置情報と画像情報との組である情報対を抽出し、
    　位置姿勢算出部が、前記情報抽出部により抽出された情報対から、前記移動体の位置及び前記移動体の姿勢のそれぞれを算出する
    　位置姿勢算出方法。
  7. 　移動体に設けられている反射材の位置を複数回測定する測定器と、
    　前記移動体に搭載されており、測量対象物を複数回撮像し、複数の撮像画像を生成する撮像器と、
    　前記測定器による複数の測定結果のそれぞれを示す位置情報を取得する位置情報取得部と、
    　前記撮像器により生成されたそれぞれの撮像画像を示す画像情報を取得する画像情報取得部と、
    　前記位置情報取得部により取得された複数の位置情報と前記画像情報取得部により取得された複数の画像情報との中から、前記測定器による位置の測定タイミングと前記撮像器による測量対象物の撮像タイミングとが同期している、位置情報と画像情報との組である情報対を抽出する情報抽出部と、
    　前記情報抽出部により抽出された情報対から、前記移動体の位置及び前記移動体の姿勢のそれぞれを算出する位置姿勢算出部と
    　前記情報抽出部により抽出された情報対に含まれている画像情報が示す撮像画像と、前記位置姿勢算出部によりそれぞれ算出された位置及び姿勢とを用いて、前記測量対象物の三次元形状を算出する三次元形状算出部と
    　を備えた測量装置。
  8. 　前記測定器は、前記反射材を追尾する機能を有していることを特徴とする請求項７記載の測量装置。
  9. 　前記撮像器は、複数のカメラを有しており、
    　前記複数のカメラの視野が互いに重複していることを特徴とする請求項７記載の測量装置。
  10. 　前記位置姿勢算出部は、前記情報抽出部により抽出された情報対に含まれている画像情報が示す撮像画像に映っているマーカの位置から、前記移動体の姿勢を算出することを特徴とする請求項７記載の測量装置。
PCT/JP2021/032573 2020-12-02 2021-09-06 位置姿勢算出装置、位置姿勢算出方法及び測量装置 WO2022118513A1 (ja)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2020-200153 2020-12-02
JP2020200153A JP2022087978A (ja) 2020-12-02 2020-12-02 位置姿勢算出装置、位置姿勢算出方法及び測量装置

Publications (1)

Publication Number Publication Date
WO2022118513A1 true WO2022118513A1 (ja) 2022-06-09

Family

ID=81854051

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2021/032573 WO2022118513A1 (ja) 2020-12-02 2021-09-06 位置姿勢算出装置、位置姿勢算出方法及び測量装置

Country Status (2)

Country Link
JP (1) JP2022087978A (ja)
WO (1) WO2022118513A1 (ja)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116242289A (zh) * 2023-05-06 2023-06-09 四川省公路规划勘察设计研究院有限公司 一种预应力锚具所具有的自探测系统及其探测方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2009002754A (ja) * 2007-06-20 2009-01-08 Pioneer Electronic Corp 停止判断装置、ナビゲーション装置、停止判断方法、停止判断プログラムおよび記録媒体
US20140163775A1 (en) * 2011-04-14 2014-06-12 Hexagon Technology Center Gmbh Geodetic marking system for marking target points
WO2017042907A1 (ja) * 2015-09-09 2017-03-16 三菱電機株式会社 航法装置および測量システム
JP2018189637A (ja) * 2017-05-01 2018-11-29 パナソニックＩｐマネジメント株式会社 カメラパラメータ算出方法、カメラパラメータ算出プログラム、カメラパラメータ算出装置、及びカメラパラメータ算出システム
JP2019056571A (ja) * 2017-09-20 2019-04-11 株式会社トプコン 測量システム
WO2020230390A1 (ja) * 2019-05-10 2020-11-19 三菱電機株式会社 位置姿勢推定装置及び位置姿勢推定方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2009002754A (ja) * 2007-06-20 2009-01-08 Pioneer Electronic Corp 停止判断装置、ナビゲーション装置、停止判断方法、停止判断プログラムおよび記録媒体
US20140163775A1 (en) * 2011-04-14 2014-06-12 Hexagon Technology Center Gmbh Geodetic marking system for marking target points
WO2017042907A1 (ja) * 2015-09-09 2017-03-16 三菱電機株式会社 航法装置および測量システム
JP2018189637A (ja) * 2017-05-01 2018-11-29 パナソニックＩｐマネジメント株式会社 カメラパラメータ算出方法、カメラパラメータ算出プログラム、カメラパラメータ算出装置、及びカメラパラメータ算出システム
JP2019056571A (ja) * 2017-09-20 2019-04-11 株式会社トプコン 測量システム
WO2020230390A1 (ja) * 2019-05-10 2020-11-19 三菱電機株式会社 位置姿勢推定装置及び位置姿勢推定方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116242289A (zh) * 2023-05-06 2023-06-09 四川省公路规划勘察设计研究院有限公司 一种预应力锚具所具有的自探测系统及其探测方法
CN116242289B (zh) * 2023-05-06 2023-07-18 四川省公路规划勘察设计研究院有限公司 一种预应力锚具所具有的自探测系统及其探测方法

Also Published As

Publication number Publication date
JP2022087978A (ja) 2022-06-14

Similar Documents

Publication Publication Date Title
US9759548B2 (en) Image processing apparatus, projector and projector system including image processing apparatus, image processing method
US8836766B1 (en) Method and system for alignment of a pattern on a spatial coded slide image
JP3868876B2 (ja) 障害物検出装置及び方法
JP5992184B2 (ja) 画像データ処理装置、画像データ処理方法および画像データ処理用のプログラム
JP5027747B2 (ja) 位置測定方法、位置測定装置、およびプログラム
WO2010001940A1 (ja) 位置測定方法、位置測定装置、およびプログラム
US20130236107A1 (en) Moving image processing device, moving image processing method, and recording medium having moving image processing program
JP2006252473A (ja) 障害物検出装置、キャリブレーション装置、キャリブレーション方法およびキャリブレーションプログラム
CN105551020B (zh) 一种检测目标物尺寸的方法及装置
JP2019510234A (ja) 奥行き情報取得方法および装置、ならびに画像取得デバイス
KR20150120066A (ko) 패턴 프로젝션을 이용한 왜곡 보정 및 정렬 시스템, 이를 이용한 방법
US20130272600A1 (en) Range image pixel matching method
US10205928B2 (en) Image processing apparatus that estimates distance information, method of controlling the same, and storage medium
WO2001004837A3 (en) Method and apparatus for detecting independent motion in three-dimensional scenes
JP2010014443A (ja) 位置測定方法、位置測定装置、およびプログラム
JP5501084B2 (ja) 平面領域検出装置及びステレオカメラシステム
JP3913901B2 (ja) カメラの内部パラメータ決定装置
WO2022118513A1 (ja) 位置姿勢算出装置、位置姿勢算出方法及び測量装置
JP3842988B2 (ja) 両眼立体視によって物体の３次元情報を計測する画像処理装置およびその方法又は計測のプログラムを記録した記録媒体
JP6243758B2 (ja) 車両周辺監視装置、車両周辺監視方法、及びプログラム
JP2003281552A (ja) 画像処理装置及びその方法
JP2009139324A (ja) 車両用走行路面検出装置
WO2020230390A1 (ja) 位置姿勢推定装置及び位置姿勢推定方法
JP2003259357A (ja) カメラのキャリブレーション方法及びカメラのアタッチメント
JP2010145219A (ja) 運動推定装置及びプログラム

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 21900269

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 21900269

Country of ref document: EP

Kind code of ref document: A1