WO2020230390A1 - 位置姿勢推定装置及び位置姿勢推定方法 - Google Patents

位置姿勢推定装置及び位置姿勢推定方法 Download PDF

Info

Publication number
WO2020230390A1
WO2020230390A1 PCT/JP2020/005938 JP2020005938W WO2020230390A1 WO 2020230390 A1 WO2020230390 A1 WO 2020230390A1 JP 2020005938 W JP2020005938 W JP 2020005938W WO 2020230390 A1 WO2020230390 A1 WO 2020230390A1
Authority
WO
WIPO (PCT)
Prior art keywords
posture
image
coordinate system
coordinates
information
Prior art date
Application number
PCT/JP2020/005938
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 WO2020230390A1 publication Critical patent/WO2020230390A1/ja

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C11/00—Photogrammetry or videogrammetry, e.g. stereogrammetry; Photographic surveying
    • G01C11/04—Interpretation of pictures
    • G01C11/06—Interpretation of pictures by comparison of two or more pictures of the same area
    • 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

  • the present disclosure relates to a position / orientation estimation device and a position / orientation estimation method.
  • a surveying device for measuring the three-dimensional shape of a surveying object (for example, the ground or a structure) using a moving body (for example, a vehicle or an aircraft) has been developed.
  • the surveying object is imaged a plurality of times by an imager (for example, a digital camera) for capturing the surveying object.
  • the position of the moving body is measured a plurality of times by a measuring device for position measurement (for example, a total station).
  • the posture of the moving body is detected a plurality of times by a posture detection sensor (for example, a gyro sensor).
  • the three-dimensional shape of the surveyed object is measured based on the captured image, the measured position, and the detected posture.
  • An imager for capturing an image of a surveyed object and a sensor for detecting a posture are provided on a moving body (see, for example, Patent Document 1).
  • the distance between the moving object and the surveying object is measured multiple times by a measuring instrument for distance measurement (for example, a laser scanner).
  • a measuring instrument for distance measurement for example, a laser scanner
  • the position of the moving body is measured a plurality of times by the measuring device for position measurement.
  • the posture of the moving body is detected a plurality of times by the posture detection sensor.
  • the three-dimensional shape of the object to be surveyed is measured based on the measured distance, the measured position, and the detected posture.
  • a measuring instrument for distance measurement and a sensor for posture detection are provided on a moving body (see, for example, Patent Document 2).
  • a sensor for attitude detection uses an image captured by an imager (image acquisition unit 30) different from the measuring device for distance measurement (distance measurement unit 20).
  • a technique for correcting the detected value by the part 40) is disclosed. This is intended to improve the accuracy of posture detection of a moving body.
  • the present disclosure has been made to solve the above-mentioned problems, and the position and posture of a moving body can be estimated with high accuracy without using a sensor for high-precision posture detection. It is an object of the present invention to provide an estimation device.
  • the position / posture estimation device is a position / posture estimation device for a moving body including a reflective material to be measured by a measuring device for position measurement and an imager, and the position measured by the measuring device.
  • the position information acquisition unit that acquires the position information indicating the position information and the image information acquisition unit that acquires the image information indicating the image captured by the imager. It is provided with a posture information generation unit that generates posture information indicating the posture of the reflective material, and a position / orientation calculation unit that calculates the position of the moving body and the posture of the moving body using the position information and the posture information.
  • the position and posture of the moving body can be estimated with high accuracy without using a sensor for high-precision posture detection.
  • FIG. 5 is a block diagram showing a main part of a computer in a surveying device including a position / orientation estimation device according to a first embodiment. It is explanatory drawing which shows the hardware composition of the computer in the surveying apparatus which includes the position and orientation estimation apparatus which concerns on Embodiment 1. FIG. It is explanatory drawing which shows the other hardware configuration of the computer in the surveying apparatus which includes the position orientation estimation apparatus which concerns on Embodiment 1.
  • FIG. It is a flowchart which shows the operation of the computer in the surveying apparatus which includes the position attitude estimation apparatus which concerns on Embodiment 1.
  • FIG. It is a flowchart which shows the detailed operation of the computer in the surveying apparatus which includes the position orientation estimation apparatus which concerns on Embodiment 1.
  • FIG. It is explanatory drawing which shows the example of the position information. It is explanatory drawing which shows the example of image information. It is explanatory drawing which shows the example of the image coordinate of each tie point in the left eye image taken at each time. It is explanatory drawing which shows the example of the image coordinate of each tie point in the right eye image taken at each time. It is explanatory drawing which shows the example of the initial value of the three-dimensional coordinates of each tie point.
  • FIG. 1 is a perspective view showing a main part of a surveying device including a position / orientation estimation device according to the first embodiment.
  • FIG. 2 is a block diagram showing a main part of a surveying device including the position / orientation estimation device according to the first embodiment.
  • FIG. 3 is a block diagram showing a main part of a computer in a surveying device including a position / orientation estimation device according to the first embodiment. A surveying device including the position / orientation estimation device according to the first embodiment will be described with reference to FIGS. 1 to 3.
  • the surveying device 200 has a moving body for surveying.
  • the moving body is composed of, for example, a carriage 1.
  • a reflective material for position measurement is provided on the trolley 1.
  • the reflective material is composed of, for example, an all-around prism 2.
  • the surveying device 200 has a measuring device for position measurement.
  • the measuring instrument is composed of, for example, an automatic tracking type total station 3. That is, the total station 3 irradiates the prism 2 with light for position measurement.
  • the prism 2 reflects the irradiated light.
  • the total station 3 receives the reflected light. As a result, the position of the prism 2 is measured.
  • the total station 3 has a function of tracking the prism 2.
  • the imager is composed of, for example, a stereo camera 4. That is, the stereo camera 4 has a right-eye camera 5 and a left-eye camera 6. The field of view taken by the right eye camera 5 overlaps with the field of view taken by the left eye camera 6. Each of the right-eye camera 5 and the left-eye camera 6 is composed of a digital camera.
  • the stereo camera 4 captures an arbitrary predetermined region (hereinafter referred to as "imaging target region").
  • the image pickup target area is, for example, an area including a survey target object by the surveying device 200.
  • the object to be surveyed is, for example, the ground.
  • an example in which the area including the survey object is set as the image pickup target area and the survey object is the ground will be mainly described.
  • the storage device 7 stores information indicating the measured position (hereinafter referred to as “position information”). At this time, each of the right-eye camera 5 and the left-eye camera 6 images the imaging target region a plurality of times at the imaging timing synchronized with the measurement timing by the total station 3.
  • the storage device 7 stores information indicating the captured image (hereinafter referred to as "image information").
  • the storage device 7 is composed of, for example, an HDD (Hard Disk Drive) or an SSD (Solid State Drive).
  • the measurement timing by the total station 3 and the imaging timing by the stereo camera 4 may be collectively referred to as "timing”.
  • 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 "stereo image” or a "digital image”.
  • the position information acquisition unit 11 acquires the position information stored in the storage device 7.
  • the image information acquisition unit 12 acquires the image information stored in the storage device 7.
  • the posture information generation unit 13 calculates the posture of the prism 2 at each timing by using the position information acquired by the position information acquisition unit 11 and the image information acquired by the image information acquisition unit 12.
  • the posture information generation unit 13 generates information indicating the calculated posture (hereinafter referred to as “posture information”).
  • the posture information generation unit 13 calculates the posture of the prism 2 based on the position of the junction point (hereinafter referred to as “tie point”) in the digital image.
  • Each tie point corresponds to, for example, a point in the imaging target area.
  • the positions of the individual points may be unknown in the surveying apparatus 200.
  • the individual tie points correspond to markers provided in the imaging target area.
  • the positions of the individual markers may be known in the surveying device 200 or unknown in the surveying device 200.
  • each tie point corresponds to a point and the position of the point is unknown
  • 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 calculates the position of the trolley 1 and the posture of the trolley 1 at each timing by using the position information acquired by the position information acquisition unit 11 and the attitude information generated by the attitude information generation unit 13. Is.
  • the three-dimensional shape calculation unit 15 calculates the three-dimensional shape of the surveyed object based on the position and posture calculated by the position / orientation calculation unit 14 using the image information acquired by the image information acquisition unit 12. is there. Specifically, for example, the three-dimensional shape calculation unit 15 calculates the three-dimensional shape of the surveyed object by so-called “multi-view stereo”. Various known techniques can be used for the multi-view stereo. Detailed description of these techniques will be omitted.
  • the main part of the computer 8 is composed of the position information acquisition unit 11, the image information acquisition unit 12, the posture information generation unit 13, the position / orientation calculation unit 14, and the three-dimensional shape calculation unit 15. Further, the position information acquisition unit 11, the image information acquisition unit 12, the posture information generation unit 13, and the position / orientation calculation unit 14 constitute a main part of the position / orientation estimation device 100.
  • the main part of the surveying device 200 is configured.
  • the computer 8 has a processor 21 and a memory 22.
  • the memory 22 stores programs for realizing the functions of the position information acquisition unit 11, the image information acquisition unit 12, the posture information generation unit 13, the position / orientation calculation unit 14, and the three-dimensional shape calculation unit 15.
  • the processor 21 reads out and executes the stored program, the functions of the position information acquisition unit 11, the image information acquisition unit 12, the posture information generation unit 13, the position / orientation calculation unit 14, and the three-dimensional shape calculation unit 15 are performed. It will be realized.
  • the computer 8 has a processing circuit 23.
  • the functions of the position information acquisition unit 11, the image information acquisition unit 12, the posture information generation unit 13, the position / orientation calculation unit 14, and the three-dimensional shape calculation unit 15 are realized by the dedicated processing circuit 23.
  • the computer 8 has a processor 21, a memory 22, and a processing circuit 23 (not shown).
  • some of the functions of the position information acquisition unit 11, the image information acquisition unit 12, the posture information generation unit 13, the position / orientation calculation unit 14, and the three-dimensional shape calculation unit 15 are realized by the processor 21 and the memory 22. Then, the remaining functions are realized by the dedicated processing circuit 23.
  • the processor 21 is composed of one or a plurality of processors.
  • a CPU Central Processing Unit
  • a GPU Graphics Processing Unit
  • a microprocessor a microcontroller
  • DSP Digital Signal Processor
  • the memory 22 is composed of one or a plurality of non-volatile memories. Alternatively, the memory 22 is composed of one or more non-volatile memories and one or more volatile memories. Each volatile memory uses, for example, a RAM (Random Access Memory).
  • the individual non-volatile memory is, for example, a ROM (Read Only Memory), a flash memory, an EPROM (Erasable Programmable Read Only Memory), an EEPROM (Electrically Erasable Programmory), or an HDD using a Memory Read-Om.
  • the processing circuit 23 is composed of one or a plurality of digital circuits. Alternatively, the processing circuit 23 is composed of one or more digital circuits and one or more analog circuits. That is, the processing circuit 23 is composed of one or a plurality of processing circuits.
  • the individual processing circuits include, for example, an ASIC (Application Specific Integrated Circuit), a PLD (Programmable Logic Device), an FPGA (Field-Programmable Gate Array), an FPGA (Field-Programmable Gate Array), and a System-System (System) System. ) Is used.
  • the world coordinate system is a right-handed three-dimensional coordinate system having X-axis, Y-axis, and Z-axis that are orthogonal to each other.
  • the XY plane in the world coordinate system is parallel or substantially parallel to the ground.
  • the positive direction of the Z axis in the world coordinate system indicates the zenith direction.
  • the prism coordinate system is a right-handed three-dimensional coordinate system having X-axis, Y-axis, and Z-axis orthogonal to each other.
  • the prism coordinate system is a coordinate system based on the prism 2.
  • the XY plane in the prism coordinate system is parallel or substantially parallel to the ground when the dolly 1 is placed on the ground.
  • the positive direction of the X-axis in the prism coordinate system indicates the front direction of the carriage 1 when the carriage 1 is placed on the ground.
  • the positive direction of the Z axis in the prism coordinate system indicates the zenith direction when the dolly 1 is placed on the ground.
  • the right-eye camera coordinate system is a right-handed three-dimensional coordinate 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 is a right-handed three-dimensional coordinate system having X-axis, Y-axis, and Z-axis orthogonal to each other.
  • 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 dolly coordinate system is an arbitrary three-dimensional coordinate system having an X-axis, a Y-axis, and a Z-axis that are orthogonal to each other.
  • the dolly coordinate system is a coordinate system based on the dolly 1.
  • the trolley coordinate system is set according to the application of the surveying device 200 and the like. Specifically, for example, the trolley coordinate system is set to a coordinate system that matches the right eye camera coordinate system. Alternatively, for example, the trolley coordinate system is set to a coordinate system that matches the left-eye camera coordinate system.
  • the origin of the prism coordinate system corresponds to the position of prism 2. Therefore, the coordinates indicating the position of the origin of the prism coordinate system in the world coordinate system indicate the position of the prism 2 in the world coordinate system. Further, the origin of the carriage coordinate system corresponds to the position of the carriage 1. Therefore, the coordinates indicating the position of the origin of the carriage coordinate system in the world coordinate system indicate the position of the carriage 1 in the world coordinate system.
  • the position coordinates in the world coordinate system, the prism coordinate system, the right eye camera coordinate system, the left eye camera coordinate system, or the trolley coordinate system are referred to as "three-dimensional coordinates".
  • the three-dimensional coordinates of individual tie points in the world coordinate system are sometimes called “tie point positions”.
  • 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 that are 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 a U-axis and a V-axis that are 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 coordinates the position coordinates in the digital image coordinate system or the normalized image coordinate system are referred to as "image coordinates”.
  • image coordinates of individual tie points in the digital image coordinate system or the normalized image coordinate system may be referred to as "tie point coordinates”.
  • the total station 3 measures the position of the prism 2 a plurality of times before the process of step ST1 is executed, and the position information is stored in the storage device 7. Further, it is assumed that the stereo camera 4 images the image pickup target area a plurality of times and the image information is stored in the storage device 7.
  • step ST1 the position information acquisition unit 11 acquires the position information stored in the storage device 7.
  • the acquired position information indicates the three-dimensional coordinates (x p , y p , z p ) of the prism 2 in the world coordinate system at each timing.
  • step ST2 the image information acquisition unit 12 acquires the image information stored in the storage device 7.
  • the acquired image information indicates a right eye image and a left eye image captured at each timing.
  • the posture information generation unit 13 calculates the posture of the prism 2 at each timing by using the position information acquired by the position information acquisition unit 11 and the image information acquired by the image information acquisition unit 12. To do. More specifically, the posture information generation unit 13 calculates the values of the posture angles ( ⁇ p , ⁇ p , ⁇ p ) of the prism 2 at each timing. As a result, posture information indicating the value of the calculated posture angle ( ⁇ p , ⁇ p , ⁇ p ) is generated.
  • the attitude angles ( ⁇ p , ⁇ p , ⁇ p ) correspond to the rotation angles of the coordinate axes of the prism coordinate system with respect to the coordinate axes of the world coordinate system. That is, the attitude angles ( ⁇ p , ⁇ p , ⁇ p ) indicate Euler angles. More specifically, ⁇ p indicates the roll angle, ⁇ p indicates the pitch angle, and ⁇ p indicates the yaw angle.
  • step ST4 the position / orientation calculation unit 14 uses the position information acquired by the position information acquisition unit 11 and the attitude information generated by the attitude information generation unit 13 to position the trolley 1 and the trolley at each timing. Calculate the posture of 1. More specifically, the position / orientation calculation unit 14 calculates the values of the three-dimensional coordinates (x t , y t , z t ) of the dolly 1 in the world coordinate system at each timing. Further, the position / orientation calculation unit 14 calculates the values of the attitude angles ( ⁇ t , ⁇ t , ⁇ t ) of the carriage 1 at each timing.
  • the attitude angles ( ⁇ t , ⁇ t , ⁇ t ) correspond to the rotation angles of the coordinate axes of the trolley coordinate system with respect to the coordinate axes of the world coordinate system. That is, the attitude angles ( ⁇ t , ⁇ t , ⁇ t ) indicate Euler angles. More specifically, ⁇ t indicates the roll angle, ⁇ t indicates the pitch angle, and ⁇ t indicates the yaw angle.
  • step ST5 the three-dimensional shape calculation unit 15 uses the image information acquired by the image information acquisition unit 12 and based on the position and orientation calculated by the position / orientation calculation unit 14, the tertiary of the surveyed object. Calculate the original shape. Specifically, for example, the three-dimensional shape calculation unit 15 calculates the three-dimensional shape of the surveyed object by the multi-view stereo.
  • step ST3 the detailed operation of the posture information generation unit 13 will be described with reference to the flowchart of FIG. That is, the detailed processing contents of step ST3 will be described.
  • step ST11 the posture information generation unit 13 determines the image coordinates (u, v) of the individual tie points in the digital image coordinate system in each of the right eye image and the left eye image captured at each timing. Calculate the value.
  • the image information acquired by the image information acquisition unit 12 is used in the process of step ST11.
  • step ST12 the attitude information generation unit 13 calculates the initial values of the three-dimensional coordinates (x T , y T , z T ) of the individual tie points in the world coordinate system.
  • the position information acquired by the position information acquisition unit 11 is used.
  • step ST13 the posture information generation unit 13 calculates the initial values of the posture angles ( ⁇ p , ⁇ p , ⁇ p ) of the prism 2 at each timing.
  • the position information acquired by the position information acquisition unit 11 is used.
  • step ST14 the posture information generation unit 13, the optimized three-dimensional coordinates relative to an initial value in step ST12 (x T, y T, z T) process for calculating the value (the "second 1 Optimization processing ”) is executed.
  • step ST15 the posture information generation unit 13, the three-dimensional coordinates calculated in step ST14 (x T, y T, z T) further than the optimized three-dimensional coordinates (x T,
  • the process of calculating the value of y T , z T ) and the value of the posture angle ( ⁇ p , ⁇ p , ⁇ p ) optimized with respect to the initial value in step ST13 (hereinafter, “second optimum”). "Chemical processing”) is executed.
  • attitude information indicating the values of the attitude angles ( ⁇ p , ⁇ p , ⁇ p ) after the optimization by the second optimization process is generated.
  • step ST1 a specific example of the position information acquired by the position information acquisition unit 11 in step ST1 will be described. Further, a specific example of the image information acquired by the image information acquisition unit 12 in step ST2 will be described with reference to FIG.
  • the total station 3 measures the position of the prism 2 a plurality of times, and the stereo camera 4 images the imaging target area a plurality of times.
  • the traveling direction of the carriage 1 is set to be the same or substantially the same as the front direction of the carriage 1.
  • FIG. 7 shows an example of position information when the position of the prism 2 is measured four times.
  • FIG. 8 shows an example of image information when the imaging target region is captured four times.
  • t0 to t3 indicate the time corresponding to the measurement timing by the total station 3, that is, the time corresponding to the imaging timing by the stereo camera 4.
  • the position information shown in FIG. 7 shows the three-dimensional coordinates (x p , y p , z p ) of the prism 2 in the world coordinate system at each time t0 to t3. Further, the image information shown in FIG. 8 shows a right eye image and a left eye image captured at each time t0 to t3.
  • each tie point corresponds to, for example, an unknown point, a known marker or an unknown marker.
  • the posture information generation unit 13 executes image processing on each digital image. As a result, the posture information generation unit 13 extracts feature points that are commonly included in the plurality of digital images. The posture information generation unit 13 uses the extracted feature points as tie points. That is, the posture information generation unit 13 calculates the values of the image coordinates (u, v) of the extracted feature points.
  • the posture information generation unit 13 executes so-called "template matching" image processing on each digital image. As a result, the posture information generation unit 13 detects the markers included in the individual digital images. The posture information generation unit 13 calculates the values of the image coordinates (u, v) of the detected marker.
  • each tie point is a known marker or an unknown marker
  • the user of the surveying apparatus 200 visually confirms the position of the marker in each digital image, and the confirmed position is determined by the computer 8. Enter in.
  • the posture information generation unit 13 calculates the value of the image coordinates (u, v) corresponding to the input position.
  • FIG. 9 shows an example of the image coordinate (u, v) value of each tie point when each tie point corresponds to an unknown marker.
  • the values shown in FIG. 9 are calculated using the image information shown in FIG.
  • the posture information generation unit 13 calculates the average value of all the X coordinates (x p ) included in the position information acquired by the position information acquisition unit 11. The posture information generation unit 13 uses the calculated average value as the initial value of the X coordinate (x T ) of each tie point.
  • the posture information generation unit 13 calculates the average value of all the Y coordinates included in the position information acquired by the position information acquisition unit 11 (y p). The posture information generation unit 13 uses the calculated average value as the initial value of the Y coordinate (y T ) of each tie point.
  • the posture information generation unit 13 calculates the value of the Z coordinate of an arbitrary point on the ground by using the value of the Z coordinate (z p ) included in the position information acquired by the position information acquisition unit 11.
  • the posture information generation unit 13 uses the calculated Z-coordinate value as the initial value of the Z-coordinate (z T ) of each tie point.
  • another prism (not shown) different from the all-around prism 2 may be provided on the carriage 1.
  • the posture information generation unit 13 may calculate the Z coordinate value of an arbitrary point on the ground based on the position of the other prism measured by the total station 3. Further, the other prism may be held by the user of the surveying device 200 instead of being provided on the carriage 1.
  • FIG. 10 shows an example of the initial values of the three-dimensional coordinates (x T , y T , z T ) of the individual tie points when the other prisms are used.
  • the initial value shown in FIG. 10 is calculated using the position information shown in FIG. 7.
  • the three-dimensional coordinates of the prism 2 at time t are (x, y, z). It is assumed that the time corresponding to the next measurement timing with respect to the measurement timing corresponding to the time t is t'. It is assumed that the three-dimensional coordinates of the prism 2 at the time t'are (x', y', z').
  • the positive direction of the X-axis in the prism coordinate system indicates the front direction of the carriage 1 when the carriage 1 is placed on the ground. Further, the traveling direction of the carriage 1 is set to be the same as or substantially the same as the front direction of the carriage 1. Therefore, the attitude angle ( ⁇ p , ⁇ p , ⁇ p ) of the prism 2 has the X coordinate (1, 0, 0) as the movement vector (xx', y-y', z-z' of the trolley 1. Approximately by the angle of rotation to match.
  • each of the roll angle ( ⁇ p ) and the pitch angle ( ⁇ p ) is approximated to 0.
  • the yaw angle ( ⁇ p ) is approximated by the angle of rotation that makes the vector (1,0) in the XY plane match the vector (xx', y-y'). Therefore, the following equation (1) holds for the yaw angle ( ⁇ p ).
  • the yaw angle ( ⁇ p ) is calculated by the above equation (1). That is, the yaw angle ( ⁇ p ) is calculated by the inverse trigonometric function.
  • the time corresponding to the previous measurement timing with respect to the measurement timing corresponding to the time t is "t". If the position of the prism 2 at a time after the time t has not been measured, the posture at the time t As the initial value of the angle ( ⁇ p , ⁇ p , ⁇ p ), the initial value of the attitude angle ( ⁇ p , ⁇ p , ⁇ p ) at time t ”is used.
  • the initial value of the attitude angle ( ⁇ p , ⁇ p , ⁇ p ) at time t ” is calculated using the three-dimensional coordinates of prism 2 at time t and the three-dimensional coordinates of prism 2 at time t”.
  • FIG. 11 shows an example of the initial values of the posture angles ( ⁇ p , ⁇ p , ⁇ p ) of the prism 2 at each time t0 to t3.
  • the initial value shown in FIG. 11 is calculated using the position information shown in FIG. 7.
  • the posture information generation unit 13 calculates the individual tie points of the projection coordinates in the right-eye camera 5 (0 u r, 0 v r). Further, the posture information generation unit 13 calculates the projected coordinates ( 0 u l , 0 v l ) of each tie point in the left eye camera 6. These projected coordinates (0 u r, 0 v r ), (0 u l, 0 v l) is the position coordinates in the normalized image coordinate system.
  • the three-dimensional coordinates of the origin of the prism coordinate system in the world coordinate system are (x p , y p , z p ). Further, it is assumed that the rotation angle of the coordinate axes of the prism coordinate system with respect to the coordinate axes of the world coordinate system is ( ⁇ p , ⁇ p , ⁇ p ). In this case, for any point X, the conversion formula between the three-dimensional coordinates X w in the world coordinate system and the three-dimensional coordinates X p in the prism coordinate system is expressed by the following formula (2).
  • the three-dimensional coordinates of the origin of the right-eye camera coordinate system in the world coordinate system are ( xr , yr , zr ).
  • the rotation angle of the coordinate axes of the right-eye camera coordinate system with respect to the coordinate axes of the world coordinate system is ( ⁇ r , ⁇ r , ⁇ r ).
  • the conversion formula between the three-dimensional coordinates X p in the world coordinate system and the three-dimensional coordinates X r in the right-eye camera coordinate system is expressed by the following formula (5).
  • the tie point having a three-dimensional coordinates corresponding to the X w, the projection coordinates in the right-eye camera 5 (0 u r, 0 v r) is calculated. That is, the focal length of the right-eye camera 5 is f r, the width of the image sensor in the left-eye camera 5 is C r, assumed width of the right-eye image is W r.
  • the conversion formula for converting the three-dimensional coordinates X w projected coordinate (0 u r, 0 v r ) is expressed by the following equation (11).
  • the three-dimensional coordinates of the origin of the left-eye camera coordinate system in the world coordinate system are (x l , y l , z l ). Further, it is assumed that the rotation angles of the coordinate axes of the left-eye camera coordinate system with respect to the coordinate axes of the world coordinate system are ( ⁇ l , ⁇ l , ⁇ l ). Further, the focal length of the left eye camera 6 is f l, the width of the imaging element in the left-eye camera 6 is C l, the width of the left-eye image is assumed to be W l.
  • the posture information generation unit 13 corrects the distortion in the image coordinates (u, v) calculated in step ST11 as follows.
  • the image coordinates of the tie points in the digital image coordinate system is assumed to be (1 u r, 1 v r ).
  • the position coordinates indicating the principal point position of the right-eye camera 5 are X pr and Y pr
  • the distortion coefficients in the right-eye camera 5 are k 1r , k 2r , k 3r , p 1r , p 2r , b 1r and b 2r .
  • the width of the image sensor in the left-eye camera 5 is C r
  • the width of the right-eye image is W r
  • height of the right eye image is assumed to be H r.
  • the tie point coordinates (1 u r, 1 v r ) having a distortion in the digital image coordinate system is converted to tie point coordinates after distortion correction in the normalized image coordinate system (2 u r, 2 v r )
  • the conversion formula is represented by the following formula (13).
  • tie points coordinates (1 u l, 1 v l ) having a distortion in a digital image coordinate system, tie point coordinates after distortion correction in the normalized image coordinate system (2 u l, 2 v l ) Is converted to.
  • the image coordinates of the tie points in the digital image coordinate system is assumed to be (1 u l, 1 v l ).
  • the position coordinates indicating the principal point position of the left-eye camera 6 are X pl and Y pl
  • the distortion coefficients in the left eye camera 6 are k 1 l , k 2 l , k 3 l , p 1 l , p 2 l , b 1 l and b 2 l .
  • the width of the imaging element in the left-eye camera 6 is C l
  • the width of the right-eye image is W l
  • height of the right eye image is assumed to be H l.
  • tie point coordinates having a distortion in the digital image coordinate system (1 u l, 1 v l ) is Thailand after distortion correction in the normalized image coordinate system It is converted to point coordinates ( 2 u l , 2 v l ).
  • the posture information generation unit 13 as described below, the difference between the calculated projected coordinates (0 u r, 0 v r ), and the calculated image coordinates (2 u r, 2 v r ) Minimize the value. Further, the posture information generation unit 13, and the calculated projected coordinates (0 u l, 0 v l ), to minimize the difference between the calculated image coordinates (2 u l, 2 v l ). This optimizes the three-dimensional coordinates of the tie points.
  • the image information acquired in step ST2 includes m right-eye images and m left-eye images.
  • the number of tie points included in the kth right eye image of the m right eye images is pk, and is included in the kth left eye image of the m left eye images. It is assumed that the number of tie points is q k .
  • n tie points T 1 , T 2 , ..., T n are detected, and the three-dimensional coordinates of n tie points T 1 , T 2 , ..., T n are respectively. It is assumed that (x T1 , y T1 , z T1 ), (x T2 , y T2 , z T2 ), ..., (X Tn , y Tn , z Tn ).
  • the function shown in the following equation (20) is defined.
  • the argument in the function is the three-dimensional coordinates of the tie point.
  • the return value in the function is the difference value between the projected coordinates of the tie point and the image coordinates of the tie point.
  • 0 ur , a, b and 0 vr , a, b are projection coordinates calculated by using the above formula (11) for the b-th tie point in the a-th right eye image. Indicates the value of. Further, 0 ul , a, b and 0 vr , a, b are the projected coordinates calculated by using the same equation as the above equation (11) for the b-th tie point in the a-th left eye image. Shows the value. Further, 2 ur , a, b and 2 vr , a, b are the image coordinates after distortion correction calculated by using the above equation (13) for the b-th tie point in the a-th right eye image.
  • 2 l, a, b and 2 v l, a, b are the b-th tie points in the a-th left eye image after distortion correction calculated by using the same formula as the above formula (13). The value of the image coordinates of is shown.
  • FIG. 12 shows a specific example of the parameter values related to the right eye camera 5 and the right eye image.
  • FIG. 13 shows a specific example of parameter values relating to the left eye camera 6 and the left eye image.
  • all the parameters excluding the three-dimensional coordinates of the tie points can be constants (see FIGS. 12 and 13). Further, when some tie points correspond to known markers, the three-dimensional coordinates of the some tie points can also be constants. As a result, it is possible to improve the calculation accuracy of the three-dimensional coordinates of the remaining tie points. However, as described above, all tie points may correspond to unknown points or unknown markers.
  • the posture information generation unit 13 executes an optimization calculation that minimizes the magnitude of the vector corresponding to the return value in the function shown in the above equation (20). As a result, the posture information generation unit 13 obtains the tie point position where the difference value between the projected coordinates and the image coordinates is minimized.
  • the optimization calculation will be described below.
  • FIG. 14 shows a value of 0 x T obtained by shaping the initial value shown in FIG. 10 (that is, the initial value calculated in step ST12).
  • the posture information generation unit 13 calculates the difference value 0 y T between the projected coordinates and the image coordinates by the following equation (22) using the initial value 0 x T of the tie point position.
  • the function shown in the above equation (20) is used for the calculation of the equation (22).
  • FIG. 15 shows the calculation result of the difference value 0 y T when the value shown in FIG. 14 is used.
  • the return value in the function shown in the above equation (20) is minimized by adding the correction amount 1 x T to the initial value 0 x T of the tie point position.
  • the calculation of the correction amount 1 x T is formulated as a problem of minimizing the residual vector V on the right side of the following equation (24) by using the gradient matrix calculated by the following equation (23).
  • FIG. 16 shows the calculation result of the gradient matrix when the values shown in FIG. 14 are used.
  • the correction amount 1 x T that minimizes the residual vector V on the right side of the above equation (24) is calculated by the least squares method based on the following equation (25).
  • FIG. 17 shows the calculation result of the difference value 0 y T shown in FIG. 15 and the calculation result of the correction amount 1 x T based on the calculation result of the gradient matrix shown in FIG.
  • N 1 indicates the number of repetitions.
  • FIG. 18 shows the value of the tie point position obtained by repeating the calculation of the least squares method three times. That is, FIG. 18 shows a specific example of the values of the three-dimensional coordinates (x T , y T , z T ) after the optimization by the first optimization process.
  • the posture information generation unit 13 calculates the individual tie points of the projection coordinates in the right-eye camera 5 (0 u r, 0 v r). Further, the posture information generation unit 13 calculates the projected coordinates ( 0 u l , 0 v l ) of each tie point in the left eye camera 6.
  • the posture information generation unit 13 corrects the distortion in the image coordinates (u, v) calculated in step ST11 by the same correction method as the correction method in the first optimization process.
  • the posture information generation unit 13 as described below, the difference between the calculated projected coordinates (0 u r, 0 v r ), and the calculated image coordinates (2 u r, 2 v r ) Minimize the value. Further, the posture information generation unit 13, and the calculated projected coordinates (0 u l, 0 v l ), to minimize the difference between the calculated image coordinates (2 u l, 2 v l ). As a result, the three-dimensional coordinates of the tie point are optimized, and the attitude angle of the prism 2 is optimized.
  • the function shown in the following equation (29) is defined.
  • the arguments in the function are the attitude angle of the prism 2 and the three-dimensional coordinates of the tie point.
  • the return value in the function is the difference value between the projected coordinates of the tie point and the image coordinates of the tie point.
  • the parameters other than the posture angle of the prism 2 and the three-dimensional coordinates of the tie points can all be constants (FIGS. 12 and 12 and). See FIG. 13). Further, when some tie points correspond to known markers, the three-dimensional coordinates of the some tie points can also be constants. As a result, the accuracy of calculating the posture angle of the prism 2 can be improved. However, as described above, all tie points may correspond to unknown points or unknown markers.
  • the posture information generation unit 13 executes an optimization calculation that minimizes the magnitude of the vector corresponding to the return value in the function shown in the above equation (29). As a result, the posture information generation unit 13 obtains the tie point position where the difference value between the projected coordinates and the image coordinates is minimized.
  • the optimization calculation will be described below.
  • FIG. 19 shows 0 x pT obtained by shaping the value shown in FIG. 18 (that is, the value calculated in step ST14) and the initial value shown in FIG. 11 (that is, the initial value calculated in step ST13). Indicates the value of.
  • the attitude information generation unit 13 calculates the difference value 0 y pT between the projected coordinates and the image coordinates by the following equation (31) using the initial values 0 x T of the attitude angle and the tie point position of the prism 2.
  • the function shown in the above equation (29) is used for the calculation of the equation (31).
  • FIG. 20 shows the calculation result of the difference value 0 y pT when the value shown in FIG. 19 is used.
  • the return value in the function shown in the above equation (29) is minimized by adding the correction amount 1 x pT to the initial value 0 x pT of the posture angle and the tie point position of the prism 2.
  • the calculation of the correction amount 1 x pT is formulated as a problem of minimizing the residual vector V on the right side of the following equation (33) by using the gradient matrix calculated by the following equation (32).
  • FIG. 21 shows the calculation result of the gradient matrix when the values shown in FIG. 19 are used.
  • the correction amount 1 x pT that minimizes the residual vector V in the above equation (32) is calculated by the least squares method based on the following equation (34).
  • FIG. 22 shows the calculation result of the difference value 0 y pT shown in FIG. 20 and the calculation result of the correction amount 1 x pT based on the calculation result of the gradient matrix shown in FIG. 21.
  • Attitude angle and tie point position can be obtained.
  • N 2 indicates the number of repetitions.
  • FIG. 23 shows the value of the attitude angle and the value of the tie point position of the prism 2 obtained by repeating the calculation of the least squares method 6 times. That is, FIG. 23 shows a specific example of the values of the three-dimensional coordinates (x T , y T , z T ) after the optimization by the second optimization process, and the posture angle ( ⁇ ) after the optimization by the second optimization process. Specific examples of the values of p , ⁇ p , ⁇ p ) are shown.
  • the dolly coordinate system When the dolly coordinate system is set to the coordinate system that matches the right-eye camera coordinate system, the three-dimensional coordinates of the dolly 1 (x t , y t , z t ) and the attitude angle of the dolly 1 ( ⁇ t , ⁇ t , ⁇ ) t ) is expressed by the following formula (38) based on the above formula (8).
  • the position / orientation calculation unit 14 uses the above equation (38) to calculate the values of the three-dimensional coordinates (x t , y t , z t ) of the carriage 1, and the attitude angle ( ⁇ t ) of the carriage 1. , ⁇ t , ⁇ t ) are calculated.
  • FIG. 24 shows a specific example of the values of the three-dimensional coordinates (x t , y t , z t ) of the trolley 1 in this case, and the specific values of the posture angles ( ⁇ t , ⁇ t , ⁇ t ) of the trolley 1. An example is shown.
  • the values shown in FIG. 24 are based on the position information shown in FIG. 7, the parameters shown in FIG. 12, and the parameters shown in FIG.
  • the position / orientation calculation unit 14 uses the same equation as the above equation (38) to make the attitude angle ( ⁇ t ) of the trolley 1. , ⁇ t , ⁇ t ), and the value of the attitude angle ( ⁇ t , ⁇ t , ⁇ t ) of the trolley 1.
  • FIG. 25 shows a specific example of the three-dimensional coordinate (x t , y t , z t ) value of the trolley 1 in this case, and the specific value of the posture angle ( ⁇ t , ⁇ t , ⁇ t ) of the trolley 1. An example is shown.
  • the values shown in FIG. 25 are based on the position information shown in FIG. 7, the parameters shown in FIG. 12, and the parameters shown in FIG.
  • the posture of the prism 2 can be calculated with high accuracy. That is, the posture of the prism 2 can be calculated with high accuracy by using the position information in addition to the image information. As a result, the posture of the dolly 1 can be estimated with high accuracy by executing the process of step ST4. In addition, the position of the carriage 1 can be estimated with high accuracy. Therefore, by executing the process of step ST5, the three-dimensional shape of the surveyed object can be measured with high accuracy.
  • the sensor for posture detection can be eliminated.
  • the surveying device 200 can be realized at low cost.
  • the moving body for surveying is not limited to the trolley 1.
  • the moving body may be a vehicle, an aircraft, a ship or a backpack carrier.
  • the reflective material for position measurement is not limited to the all-around prism 2.
  • the reflective material may be one using an ordinary prism.
  • the measuring instrument for position measurement is not limited to the automatic tracking type total station 3.
  • the measuring instrument may be one using a normal total station having no automatic tracking function.
  • the imager is not limited to the stereo camera 4 using two digital cameras.
  • the imager may be one using one or more digital cameras. However, by increasing the number of digital cameras, it is possible to further improve the estimation accuracy of the position and orientation of the carriage 1. On the other hand, by reducing the number of digital cameras, the surveying device 200 can be realized at a lower cost.
  • the imaging target area is not limited to the area including the survey object.
  • 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 survey object may include a horizontal plane, a vertical plane, or an inclined surface. Further, the survey object may be flat, curved, or uneven.
  • the moving body When a region different from the region including the survey object is set as the imaging target region, the moving body may be provided with a measuring device for distance measurement (for example, a laser scanner).
  • the three-dimensional shape calculation unit 15 calculates the three-dimensional shape of the object to be measured based on the distance measured by the measuring instrument, the position calculated by the position / orientation calculation unit 14, and the posture calculated by the position / orientation calculation unit 14. It may be calculated.
  • the posture information generation unit 13 may execute the second optimization process without executing the first optimization process. That is, the posture information generation unit 13 may execute the process of step ST15 after step ST13.
  • the second optimization process in this case, the values of the three-dimensional coordinates (x T , y T , z T ) optimized with respect to the initial values in step ST12 are calculated and compared with the initial values in step ST13. It may be a process of calculating the value of the optimized posture angle ( ⁇ p , ⁇ p , ⁇ p ).
  • the posture of the prism 2 can be accurately calculated even when the difference value with respect to the true value of the initial value in step ST12 is large. Therefore, it is more preferable to execute the first optimization process.
  • the position / posture estimation device 100 is the position / posture estimation device 100 for a moving body including a reflective material to be measured by the measuring device for position measurement and an imager.
  • the position information acquisition unit 11 that acquires the position information indicating the position measured by the measuring instrument
  • the image information acquisition unit 12 that acquires the image information indicating the image captured by the imager
  • the position information and the image information The posture information generation unit 13 that generates posture information indicating the posture of the reflector by calculating the posture of the reflective material using the posture information, and the position of the moving body and the posture of the moving body are calculated using the position information and the posture information.
  • a position / posture calculation unit 14 is provided. As a result, the position and posture of the moving body can be estimated with high accuracy without using a sensor for high-precision posture detection.
  • the position / posture estimation method is a position / posture estimation method for a moving body including a reflective material to be measured by a measuring device for position measurement and an imager, and obtains position information.
  • Step ST1 in which the unit 11 acquires position information indicating the position measured by the measuring instrument
  • step ST2 in which the image information acquisition unit 12 acquires image information indicating the image captured by the imager
  • posture information generation in which the unit 13 calculates the posture of the reflective material using the position information and the image information to generate the posture information indicating the posture of the reflective material
  • the position / attitude calculation unit 14 generate the position information and the posture information. It includes step ST4 of calculating the position of the moving body and the posture of the moving body by using the step ST4.
  • the position and posture of the moving body can be estimated with high accuracy without using a sensor for high-precision posture detection.
  • the position / orientation estimation device and the position / orientation estimation method according to the present disclosure can be used, for example, for measuring the three-dimensional shape of the ground or a structure.
  • 1 trolley (moving body), 2 prism (reflecting material), 3 total station (measuring instrument), 4 stereo camera (imaging machine), 5 right eye camera, 6 left eye camera, 7 storage device, 8 computer, 11 position information acquisition Unit, 12 image information acquisition unit, 13 attitude information generation unit, 14 position / orientation calculation unit, 15 three-dimensional shape calculation unit, 21 processor, 22 memory, 23 processing circuit, 100 position / orientation estimation device, 200 surveying device.

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Multimedia (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
　ただし、以下の式（９）及び式（１０）が成立している。

Figure JPOXMLDOC01-appb-I000009

Figure JPOXMLDOC01-appb-I000010
　上記式（８）に基づき、Ｘｗに対応する三次元座標を有するタイポイントについて、右眼カメラ５における投影座標（０ｕｒ，０ｖｒ）が算出される。すなわち、右眼カメラ５の焦点距離がｆｒであり、右眼カメラ５における撮像素子の幅がＣｒであり、右眼画像の横幅がＷｒであるものとする。このとき、三次元座標Ｘｗを投影座標（０ｕｒ，０ｖｒ）に変換する変換式は、以下の式（１１）により表される。

Figure JPOXMLDOC01-appb-I000011
　ただし、以下の式（１２）が成立している。

Figure JPOXMLDOC01-appb-I000012
　上記式（１１）を用いて三次元座標Ｘｗが投影座標（０ｕｒ，０ｖｒ）に変換されることにより、右眼カメラ５における投影座標（０ｕｒ，０ｖｒ）が算出される。これと同様にして、左眼カメラ６における投影座標（０ｕｌ，０ｖｌ）が算出される。
　すなわち、世界座標系における左眼カメラ座標系の原点の三次元座標が（ｘｌ，ｙｌ，ｚｌ）であるものとする。また、世界座標系の座標軸に対する左眼カメラ座標系の座標軸の回転角が（ωｌ，φｌ，κｌ）であるものとする。また、左眼カメラ６の焦点距離がｆｌであり、左眼カメラ６における撮像素子の幅がＣｌであり、左眼画像の横幅がＷｌであるものとする。これらの値に基づき、三次元座標Ｘｗを投影座標（０ｕｌ，０ｖｌ）に変換する変換式を用いて、左眼カメラ６における投影座標（０ｕｌ，０ｖｌ）が算出される。
　次いで、姿勢情報生成部１３は、以下のようにして、ステップＳＴ１１にて算出された画像座標（ｕ，ｖ）における歪みを補正する。
　右眼画像において、デジタル画像座標系におけるタイポイントの画像座標が（１ｕｒ，１ｖｒ）であるものとする。右眼カメラ５の主点位置を示す位置座標がＸｐｒ及びＹｐｒであり、右眼カメラ５における歪み係数がｋ１ｒ、ｋ２ｒ、ｋ３ｒ、ｐ１ｒ、ｐ２ｒ、ｂ１ｒ及びｂ２ｒであり、右眼カメラ５における撮像素子の幅がＣｒであり、右眼画像の横幅がＷｒであり、右眼画像の縦幅がＨｒであるものとする。このとき、デジタル画像座標系における歪みを有するタイポイント座標（１ｕｒ，１ｖｒ）を、正規化画像座標系における歪み補正後のタイポイント座標（２ｕｒ，２ｖｒ）に変換する変換式は、以下の式（１３）により表される。

Figure JPOXMLDOC01-appb-I000013
　ただし、以下の式（１４）～式（１９）が成立している。

Figure JPOXMLDOC01-appb-I000014

Figure JPOXMLDOC01-appb-I000015

Figure JPOXMLDOC01-appb-I000016

Figure JPOXMLDOC01-appb-I000017

Figure JPOXMLDOC01-appb-I000018

Figure JPOXMLDOC01-appb-I000019
　上記式（１３）を用いて、デジタル画像座標系における歪みを有するタイポイント座標（１ｕｒ，１ｖｒ）が、正規化画像座標系における歪み補正後のタイポイント座標（２ｕｒ，２ｖｒ）に変換される。これと同様にして、デジタル画像座標系における歪みを有するタイポイント座標（１ｕｌ，１ｖｌ）が、正規化画像座標系における歪み補正後のタイポイント座標（２ｕｌ，２ｖｌ）に変換される。
　すなわち、左眼画像において、デジタル画像座標系におけるタイポイントの画像座標が（１ｕｌ，１ｖｌ）であるものとする。左眼カメラ６の主点位置を示す位置座標がＸｐｌ及びＹｐｌであり、左眼カメラ６における歪み係数がｋ１ｌ、ｋ２ｌ、ｋ３ｌ、ｐ１ｌ、ｐ２ｌ、ｂ１ｌ及びｂ２ｌであり、左眼カメラ６における撮像素子の幅がＣｌであり、右眼画像の横幅がＷｌであり、右眼画像の縦幅がＨｌであるものとする。このとき、上記式（１３）と同様の変換式を用いて、デジタル画像座標系における歪みを有するタイポイント座標（１ｕｌ，１ｖｌ）が、正規化画像座標系における歪み補正後のタイポイント座標（２ｕｌ，２ｖｌ）に変換される。
　次いで、姿勢情報生成部１３は、以下のようにして、上記算出された投影座標（０ｕｒ，０ｖｒ）と、上記算出された画像座標（２ｕｒ，２ｖｒ）との差分値を最小化する。また、姿勢情報生成部１３は、上記算出された投影座標（０ｕｌ，０ｖｌ）と、上記算出された画像座標（２ｕｌ，２ｖｌ）との差分値を最小化する。これにより、タイポイントの三次元座標が最適化される。
　ステップＳＴ２にて取得された画像情報に、ｍ枚の右眼画像及びｍ枚の左眼画像が含まれているものとする。ｍ枚の右眼画像のうちのｋ番目の右眼画像に含まれているタイポイントの個数がｐｋ個であり、ｍ枚の左眼画像のうちのｋ番目の左眼画像に含まれているタイポイントの個数がｑｋ個であるものとする。また、ステップＳＴ１１にてｎ個のタイポイントＴ１，Ｔ２，……，Ｔｎが検出されており、ｎ個のタイポイントＴ１，Ｔ２，……，Ｔｎの三次元座標がそれぞれ（ｘＴ１，ｙＴ１，ｚＴ１），（ｘＴ２，ｙＴ２，ｚＴ２），……，（ｘＴｎ，ｙＴｎ，ｚＴｎ）であるものとする。
　上記式（１１）及び上記式（１３）に基づき、以下の式（２０）に示す関数が定義される。当該関数における引数は、タイポイントの三次元座標である。当該関数における返り値は、タイポイントの投影座標とタイポイントの画像座標との差分値である。

Figure JPOXMLDOC01-appb-I000020
　上記式（２０）において、０ｕｒ，ａ，ｂ及び０ｖｒ，ａ，ｂは、ａ番目の右眼画像におけるｂ番目のタイポイントについて上記式（１１）を用いて算出された投影座標の値を示している。また、０ｕｌ，ａ，ｂ及び０ｖｌ，ａ，ｂは、ａ番目の左眼画像におけるｂ番目のタイポイントについて上記式（１１）と同様の式を用いて算出された投影座標の値を示している。また、２ｕｒ，ａ，ｂ及び２ｖｒ，ａ，ｂは、ａ番目の右眼画像におけるｂ番目のタイポイントについて上記式（１３）を用いて算出された歪み補正後の画像座標の値を示している。また、２ｕｌ，ａ，ｂ及び２ｖｌ，ａ，ｂは、ａ番目の左眼画像におけるｂ番目のタイポイントについて上記式（１３）と同様の式を用いて算出された歪み補正後の画像座標の値を示している。
　ここで、図１２は、右眼カメラ５及び右眼画像に関するパラメータの値の具体例を示している。図１３は、左眼カメラ６及び左眼画像に関するパラメータの値の具体例を示している。これらのパラメータは、上記式（１１）及び上記式（１３）に基づく上記式（２０）の計算に用いられるものである。
　上記式（１１）及び上記（１３）に基づく上記式（２０）の計算において、タイポイントの三次元座標を除くパラメータは、いずれも定数とすることができる（図１２及び図１３参照）。また、一部のタイポイントが既知のマーカに対応するものである場合、当該一部のタイポイントの三次元座標も定数とすることができる。これにより、残余のタイポイントの三次元座標の算出精度の向上を図ることができる。ただし、上記のとおり、全てのタイポイントが未知の地点又は未知のマーカに対応するものであっても良い。
　姿勢情報生成部１３は、上記式（２０）に示す関数において、返り値に対応するベクトルの大きさを最小にする最適化計算を実行する。これにより、姿勢情報生成部１３は、投影座標と画像座標との差分値が最小となるタイポイント位置を求める。以下、かかる最適化計算について説明する。
　まず、タイポイント位置の初期値０ｘＴが以下の式（２１）により表されるものとする。図１４は、図１０に示す初期値（すなわちステップＳＴ１２にて算出された初期値）を整形することにより得られた０ｘＴの値を示している。

Figure JPOXMLDOC01-appb-I000021
　姿勢情報生成部１３は、タイポイント位置の初期値０ｘＴを用いて、以下の式（２２）により、投影座標と画像座標との差分値０ｙＴを算出する。式（２２）の計算には、上記式（２０）に示す関数が用いられる。図１５は、図１４に示す値を用いた場合における差分値０ｙＴの算出結果を示している。

Figure JPOXMLDOC01-appb-I000022
　ここで、タイポイント位置の初期値０ｘＴに補正量１ｘＴを加えることにより、上記式（２０）に示す関数における返り値を最小化することを考える。補正量１ｘＴの算出は、以下の式（２３）により算出される勾配行列を用いて、以下の式（２４）の右辺における残差ベクトルＶを最小化する問題として定式化される。図１６は、図１４に示す値を用いた場合における勾配行列の算出結果を示している。

Figure JPOXMLDOC01-appb-I000023

Figure JPOXMLDOC01-appb-I000024
　上記式（２４）の右辺における残差ベクトルＶを最小化する補正量１ｘＴは、以下の式（２５）に基づき、最小二乗法により算出される。図１７は、図１５に示す差分値０ｙＴの算出結果及び図１６に示す勾配行列の算出結果に基づく補正量１ｘＴの算出結果を示している。

Figure JPOXMLDOC01-appb-I000025
　ただし、以下の式（２６）が成立している。

Figure JPOXMLDOC01-appb-I000026
　このようにして、以下の式（２７）を満たす補正量１ｘＴが算出される。

Figure JPOXMLDOC01-appb-I000027
　得られたタイポイント位置に対して最小二乗法を繰り返し適用することにより、次段落の（２８）に示す如く、投影座標と画像座標との差分値が最小となるタイポイント位置が得られる。ここで、Ｎ１は繰り返し回数を示している。

Figure JPOXMLDOC01-appb-I000028
　図１８は、最小二乗法の計算を３回繰り返すことにより得られたタイポイント位置の値を示している。すなわち、図１８は、第１最適化処理による最適化後の三次元座標（ｘＴ，ｙＴ，ｚＴ）の値の具体例を示している。
　次に、図１９～図２３を参照して、ステップＳＴ１５における、姿勢情報生成部１３による第２最適化処理の具体例について説明する。
　まず、姿勢情報生成部１３は、第１最適化処理における算出方法と同様の算出方法により、右眼カメラ５における個々のタイポイントの投影座標（０ｕｒ，０ｖｒ）を算出する。また、姿勢情報生成部１３は、左眼カメラ６における個々のタイポイントの投影座標（０ｕｌ，０ｖｌ）を算出する。
　次いで、姿勢情報生成部１３は、第１最適化処理における補正方法と同様の補正方法により、ステップＳＴ１１にて算出された画像座標（ｕ，ｖ）における歪みを補正する。
　次いで、姿勢情報生成部１３は、以下のようにして、上記算出された投影座標（０ｕｒ，０ｖｒ）と、上記算出された画像座標（２ｕｒ，２ｖｒ）との差分値を最小化する。また、姿勢情報生成部１３は、上記算出された投影座標（０ｕｌ，０ｖｌ）と、上記算出された画像座標（２ｕｌ，２ｖｌ）との差分値を最小化する。これにより、タイポイントの三次元座標が最適化されるとともに、プリズム２の姿勢角度が最適化される。
　上記式（１１）及び上記式（１３）に基づき、以下の式（２９）に示す関数が定義される。当該関数における引数は、プリズム２の姿勢角度及びタイポイントの三次元座標である。当該関数における返り値は、タイポイントの投影座標とタイポイントの画像座標との差分値である。

Figure JPOXMLDOC01-appb-I000029
　上記式（２９）において、上記式（２０）に示す符号と同様の符号は、上記（２０）におけるパラメータと同様のパラメータを示している。また、（ωｐｋ，φｐｋ，κｐｋ）は、ｋ番目の右眼画像の撮像タイミングに対応する測定タイミングにおけるプリズム２の姿勢角度を示している。
　上記式（１１）及び上記（１３）に基づく上記式（２９）の計算において、プリズム２の姿勢角度及びタイポイントの三次元座標を除くパラメータは、いずれも定数とすることができる（図１２及び図１３参照）。また、一部のタイポイントが既知のマーカに対応するものである場合、当該一部のタイポイントの三次元座標も定数とすることができる。これにより、プリズム２の姿勢角度の算出精度の向上を図ることができる。ただし、上記のとおり、全てのタイポイントが未知の地点又は未知のマーカに対応するものであっても良い。
　姿勢情報生成部１３は、上記式（２９）に示す関数において、返り値に対応するベクトルの大きさを最小にする最適化計算を実行する。これにより、姿勢情報生成部１３は、投影座標と画像座標との差分値が最小となるタイポイント位置を求める。以下、かかる最適化計算について説明する。
　まず、プリズム２の姿勢角度及びタイポイント位置の初期値０ｘｐＴが以下の式（３０）により表されるものとする。図１９は、図１８に示す値（すなわちステップＳＴ１４にて算出された値）及び図１１に示す初期値（すなわちステップＳＴ１３にて算出された初期値）を整形することにより得られた０ｘｐＴの値を示している。

Figure JPOXMLDOC01-appb-I000030
　姿勢情報生成部１３は、プリズム２の姿勢角度及びタイポイント位置の初期値０ｘＴを用いて、以下の式（３１）により、投影座標と画像座標との差分値０ｙｐＴを算出する。式（３１）の計算には、上記式（２９）に示す関数が用いられる。図２０は、図１９に示す値を用いた場合における差分値０ｙｐＴの算出結果を示している。

Figure JPOXMLDOC01-appb-I000031
　ここで、プリズム２の姿勢角度及びタイポイント位置の初期値０ｘｐＴに補正量１ｘｐＴを加えることにより、上記式（２９）に示す関数における返り値を最小化することを考える。補正量１ｘｐＴの算出は、以下の式（３２）により算出される勾配行列を用いて、以下の式（３３）の右辺における残差ベクトルＶを最小化する問題として定式化される。図２１は、図１９に示す値を用いた場合における勾配行列の算出結果を示している。

Figure JPOXMLDOC01-appb-I000032

Figure JPOXMLDOC01-appb-I000033
　上記式（３２）における残差ベクトルＶを最小化する補正量１ｘｐＴは、以下の式（３４）に基づき、最小二乗法により算出される。図２２は、図２０に示す差分値０ｙｐＴの算出結果及び図２１に示す勾配行列の算出結果に基づく補正量１ｘｐＴの算出結果を示している。

Figure JPOXMLDOC01-appb-I000034
　ただし、以下の式（３５）が成立している。

Figure JPOXMLDOC01-appb-I000035
　このようにして、以下の式（３６）を満たす補正量１ｘｐＴが算出される。

Figure JPOXMLDOC01-appb-I000036
　得られたプリズム２の姿勢角度及びタイポイント位置に対して最小二乗法を繰り返し適用することにより、次段落の（３７）に示す如く、投影座標と画像座標との差分値が最小となるプリズム２の姿勢角度及びタイポイント位置が得られる。ここで、Ｎ２は繰り返し回数を示している。

Figure JPOXMLDOC01-appb-I000037
　図２３は、最小二乗法の計算を６回繰り返すことにより得られたプリズム２の姿勢角度の値及びタイポイント位置の値を示している。すなわち、図２３は、第２最適化処理による最適化後の三次元座標（ｘＴ，ｙＴ，ｚＴ）の値の具体例、及び第２最適化処理による最適化後の姿勢角度（ωｐ，φｐ，κｐ）の値の具体例を示している。
　次に、図２４及び図２５を参照して、ステップＳＴ４における、位置姿勢算出部１４による台車１の三次元座標（ｘｔ，ｙｔ，ｚｔ）の値の算出方法の具体例について説明する。また、台車１の姿勢角度（ωｔ，φｔ，κｔ）の値の算出方法の具体例について説明する。
　台車座標系が右眼カメラ座標系と一致する座標系に設定される場合、台車１の三次元座標（ｘｔ，ｙｔ，ｚｔ）及び台車１の姿勢角度（ωｔ，φｔ，κｔ）は、上記式（８）に基づき、以下の式（３８）により表される。

Figure JPOXMLDOC01-appb-I000038
　ただし、以下の式（３９）及び式（４０）が成立している。

Figure JPOXMLDOC01-appb-I000039

Figure JPOXMLDOC01-appb-I000040
　この場合、位置姿勢算出部１４は、上記式（３８）を用いて、台車１の三次元座標（ｘｔ，ｙｔ，ｚｔ）の値を算出するとともに、台車１の姿勢角度（ωｔ，φｔ，κｔ）の値を算出する。図２４は、この場合における、台車１の三次元座標（ｘｔ，ｙｔ，ｚｔ）の値の具体例、及び台車１の姿勢角度（ωｔ，φｔ，κｔ）の値の具体例を示している。なお、図２４に示す値は、図７に示す位置情報、図１２に示すパラメータ、及び図１３に示すパラメータに基づくものである。
　他方、台車座標系が左眼カメラ座標系と一致する座標系に設定される場合、位置姿勢算出部１４は、上記式（３８）と同様の式を用いて、台車１の姿勢角度（ωｔ，φｔ，κｔ）の値を算出するとともに、台車１の姿勢角度（ωｔ，φｔ，κｔ）の値を算出する。図２５は、この場合における、台車１の三次元座標（ｘｔ，ｙｔ，ｚｔ）の値の具体例、及び台車１の姿勢角度（ωｔ，φｔ，κｔ）の値の具体例を示している。なお、図２５に示す値は、図７に示す位置情報、図１２に示すパラメータ、及び図１３に示すパラメータに基づくものである。
　このように、ステップＳＴ１～ＳＴ３の処理が実行されることにより、プリズム２の姿勢を高精度に算出することができる。すなわち、画像情報に加えて位置情報を用いることにより、プリズム２の姿勢を高精度に算出することができる。この結果、ステップＳＴ４の処理が実行されることにより、台車１の姿勢を高精度に推定することができる。また、台車１の位置を高精度に推定することができる。したがって、ステップＳＴ５の処理が実行されることにより、測量対象物の三次元形状を高精度に測定することができる。
　また、姿勢検出用のセンサを不要とすることができる。この結果、測量装置２００を安価に実現することができる。
　次に、測量装置２００の変形例について説明する。
　測量用の移動体は、台車１に限定されるものではない。例えば、移動体は、車両、航空機、船舶又は背負子であっても良い。
　位置測定用の反射材は、全周型のプリズム２に限定されるものではない。例えば、トータルステーション３に対する移動体の相対方向の変動量が小さい場合、反射材は、通常のプリズムを用いたものであっても良い。
　位置測定用の測定器は、自動追尾型のトータルステーション３に限定されるものではない。例えば、各タイミングにて移動体が静止するものである場合、測定器は、自動追尾機能を有しない通常のトータルステーションを用いたものであっても良い。
　撮像機は、２個のデジタルカメラを用いたステレオカメラ４に限定されるものではない。撮像機は、１個以上のデジタルカメラを用いたものであれば良い。ただし、デジタルカメラの個数を増やすことにより、台車１の位置及び姿勢の推定精度を更に向上することができる。他方、デジタルカメラの個数を減らすことにより、測量装置２００を更に安価に実現することができる。
　撮像対象領域は、測量対象物を含む領域に限定されるものではない。また、測量対象物は、地面に限定されるものではない。例えば、測量対象物は、橋梁の表面又はトンネルの内面であっても良い。すなわち、測量対象物は、水平面を含むものであっても良く、鉛直面を含むものであっても良く、傾斜面を含むものであっても良い。また、測量対象物は、平面状であっても良く、曲面状であっても良く、凹凸面状であっても良い。
　なお、測量対象物を含む領域と異なる領域が撮像対象領域に設定されている場合、移動体に距離測定用の測定器（例えばレーザスキャナ）が設けられているものであっても良い。三次元形状算出部１５は、当該測定器により測定された距離、位置姿勢算出部１４により算出された位置、及び位置姿勢算出部１４により算出された姿勢に基づき、測量対象物の三次元形状を算出するものであっても良い。
　姿勢情報生成部１３は、第１最適化処理を実行することなく、第２最適化処理を実行するものであっても良い。すなわち、姿勢情報生成部１３は、ステップＳＴ１３に次いで、ステップＳＴ１５の処理を実行するものであっても良い。この場合における第２最適化処理は、ステップＳＴ１２における初期値に比して最適化された三次元座標（ｘＴ，ｙＴ，ｚＴ）の値を算出するとともに、ステップＳＴ１３における初期値に比して最適化された姿勢角度（ωｐ，φｐ，κｐ）の値を算出する処理であっても良い。
　ただし、第１最適化処理が実行されることにより、ステップＳＴ１２における初期値の真値に対する差分値が大きいときであっても、プリズム２の姿勢を正確に算出することができる。このため、第１最適化処理を実行するのがより好適である。
　以上のように、実施の形態１に係る位置姿勢推定装置１００は、位置測定用の測定器による測定対象となる反射材と、撮像機と、を備える移動体用の位置姿勢推定装置１００であって、測定器により測定された位置を示す位置情報を取得する位置情報取得部１１と、撮像機により撮像された画像を示す画像情報を取得する画像情報取得部１２と、位置情報及び画像情報を用いて反射材の姿勢を算出することにより、反射材の姿勢を示す姿勢情報を生成する姿勢情報生成部１３と、位置情報及び姿勢情報を用いて移動体の位置及び移動体の姿勢を算出する位置姿勢算出部１４と、を備える。これにより、高精度な姿勢検出用のセンサを用いることなく、移動体の位置及び姿勢を高精度に推定することができる。
　また、実施の形態１に係る位置姿勢推定方法は、位置測定用の測定器による測定対象となる反射材と、撮像機と、を備える移動体用の位置姿勢推定方法であって、位置情報取得部１１が、測定器により測定された位置を示す位置情報を取得するステップＳＴ１と、画像情報取得部１２が、撮像機により撮像された画像を示す画像情報を取得するステップＳＴ２と、姿勢情報生成部１３が、位置情報及び画像情報を用いて反射材の姿勢を算出することにより、反射材の姿勢を示す姿勢情報を生成するステップＳＴ３と、位置姿勢算出部１４が、位置情報及び姿勢情報を用いて移動体の位置及び移動体の姿勢を算出するステップＳＴ４と、を備える。これにより、高精度な姿勢検出用のセンサを用いることなく、移動体の位置及び姿勢を高精度に推定することができる。
　なお、本願開示はその開示の範囲内において、実施の形態の任意の構成要素の変形、もしくは実施の形態の任意の構成要素の省略が可能である。
　本開示に係る位置姿勢推定装置及び位置姿勢推定方法は、例えば、地面又は構造物の三次元形状の測定に用いることができる。
　１　台車（移動体）、２　プリズム（反射材）、３　トータルステーション（測定器）、４　ステレオカメラ（撮像機）、５　右眼カメラ、６　左眼カメラ、７　記憶装置、８　計算機、１１　位置情報取得部、１２　画像情報取得部、１３　姿勢情報生成部、１４　位置姿勢算出部、１５　三次元形状算出部、２１　プロセッサ、２２　メモリ、２３　処理回路、１００　位置姿勢推定装置、２００　測量装置。

Claims (5)

  1. 　位置測定用の測定器による測定対象となる反射材と、撮像機と、を備える移動体用の位置姿勢推定装置であって、
    　前記測定器により測定された位置を示す位置情報を取得する位置情報取得部と、
    　前記撮像機により撮像された画像を示す画像情報を取得する画像情報取得部と、
    　前記位置情報及び前記画像情報を用いて前記反射材の姿勢を算出することにより、前記反射材の姿勢を示す姿勢情報を生成する姿勢情報生成部と、
    　前記位置情報及び前記姿勢情報を用いて前記移動体の位置及び前記移動体の姿勢を算出する位置姿勢算出部と、
    　を備えることを特徴とする位置姿勢推定装置。
  2. 　前記測定器は、前記反射材を追尾する機能を有することを特徴とする請求項１記載の位置姿勢推定装置。
  3. 　前記撮像機は、複数個のカメラにより構成されており、
    　前記複数個のカメラによる視野が互いに重複している
    　ことを特徴とする請求項１記載の位置姿勢推定装置。
  4. 　前記姿勢情報生成部は、前記画像に含まれるマーカの位置に基づき前記反射材の姿勢を算出するものであり、
    　前記マーカの位置が既知である
    　ことを特徴とする請求項１記載の位置姿勢推定装置。
  5. 　位置測定用の測定器による測定対象となる反射材と、撮像機と、を備える移動体用の位置姿勢推定方法であって、
    　位置情報取得部が、前記測定器により測定された位置を示す位置情報を取得するステップと、
    　画像情報取得部が、前記撮像機により撮像された画像を示す画像情報を取得するステップと、
    　姿勢情報生成部が、前記位置情報及び前記画像情報を用いて前記反射材の姿勢を算出することにより、前記反射材の姿勢を示す姿勢情報を生成するステップと、
    　位置姿勢算出部が、前記位置情報及び前記姿勢情報を用いて前記移動体の位置及び前記移動体の姿勢を算出するステップと、
    　を備えることを特徴とする位置姿勢推定方法。
PCT/JP2020/005938 2019-05-10 2020-02-17 位置姿勢推定装置及び位置姿勢推定方法 WO2020230390A1 (ja)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2019-089947 2019-05-10
JP2019089947A JP2020186944A (ja) 2019-05-10 2019-05-10 位置姿勢推定装置及び位置姿勢推定方法

Publications (1)

Publication Number Publication Date
WO2020230390A1 true WO2020230390A1 (ja) 2020-11-19

Family

ID=73221579

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2020/005938 WO2020230390A1 (ja) 2019-05-10 2020-02-17 位置姿勢推定装置及び位置姿勢推定方法

Country Status (2)

Country Link
JP (1) JP2020186944A (ja)
WO (1) WO2020230390A1 (ja)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114494388A (zh) * 2022-01-27 2022-05-13 中国铁建重工集团股份有限公司 一种大视场环境下图像三维重建方法、装置、设备及介质
WO2022118513A1 (ja) * 2020-12-02 2022-06-09 三菱電機株式会社 位置姿勢算出装置、位置姿勢算出方法及び測量装置

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2002010297A (ja) * 2000-06-26 2002-01-11 Topcon Corp ステレオ画像撮影システム
US20140032021A1 (en) * 2011-04-14 2014-01-30 Hexagon Technology Center Gmbh System and method for controlling an unmanned air vehicle
JP2016045150A (ja) * 2014-08-26 2016-04-04 株式会社トプコン 点群位置データ処理装置、点群位置データ処理システム、点群位置データ処理方法およびプログラム
JP2019045425A (ja) * 2017-09-06 2019-03-22 株式会社トプコン 測量データ処理装置、測量データ処理方法および測量データ処理用プログラム

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2002010297A (ja) * 2000-06-26 2002-01-11 Topcon Corp ステレオ画像撮影システム
US20140032021A1 (en) * 2011-04-14 2014-01-30 Hexagon Technology Center Gmbh System and method for controlling an unmanned air vehicle
JP2016045150A (ja) * 2014-08-26 2016-04-04 株式会社トプコン 点群位置データ処理装置、点群位置データ処理システム、点群位置データ処理方法およびプログラム
JP2019045425A (ja) * 2017-09-06 2019-03-22 株式会社トプコン 測量データ処理装置、測量データ処理方法および測量データ処理用プログラム

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2022118513A1 (ja) * 2020-12-02 2022-06-09 三菱電機株式会社 位置姿勢算出装置、位置姿勢算出方法及び測量装置
CN114494388A (zh) * 2022-01-27 2022-05-13 中国铁建重工集团股份有限公司 一种大视场环境下图像三维重建方法、装置、设备及介质

Also Published As

Publication number Publication date
JP2020186944A (ja) 2020-11-19

Similar Documents

Publication Publication Date Title
US10823552B2 (en) Method for the three dimensional measurement of moving objects during a known movement
JP4814669B2 (ja) ３次元座標取得装置
US9733339B2 (en) Position and orientation calibration method and apparatus
KR102085228B1 (ko) 깊이 센서의 깊이 보정을 위한 이미지 처리방법 및 그 장치
JP3859574B2 (ja) ３次元視覚センサ
US8917942B2 (en) Information processing apparatus, information processing method, and program
US11544860B2 (en) Combined point cloud generation using a stationary laser scanner and a mobile scanner
US20130108116A1 (en) Position/orientation measurement apparatus, measurement processing method thereof, and non-transitory computer-readable storage medium
CN110044374B (zh) 一种基于图像特征的单目视觉测量里程的方法及里程计
KR101614338B1 (ko) 카메라 캘리브레이션 방법, 카메라 캘리브레이션 프로그램을 기록한 컴퓨터 판독가능한 기록매체 및 카메라 캘리브레이션 장치
KR20160003776A (ko) 자세 추정 방법 및 로봇
JP2016045150A (ja) 点群位置データ処理装置、点群位置データ処理システム、点群位置データ処理方法およびプログラム
JP2013186816A (ja) 動画処理装置、動画処理方法および動画処理用のプログラム
JP2009027700A (ja) 移動体搭載用前方撮像制御装置
WO2020230390A1 (ja) 位置姿勢推定装置及び位置姿勢推定方法
US9914222B2 (en) Information processing apparatus, control method thereof, and computer readable storage medium that calculate an accuracy of correspondence between a model feature and a measurement data feature and collate, based on the accuracy, a geometric model and an object in an image
KR101592405B1 (ko) 3차원 영상 획득 방법, 장치 및 컴퓨터 판독 가능한 기록 매체
US20130021449A1 (en) Three-dimensional measuring method
JP5858773B2 (ja) ３次元計測方法、３次元計測プログラム及びロボット装置
JP2010066595A (ja) 環境地図生成装置及び環境地図生成方法
JP6536529B2 (ja) 車載カメラのキャリブレーション装置及び車載カメラのキャリブレーション方法
CN110736426B (zh) 物体尺寸获取方法、装置、计算机设备以及存储介质
JP2015007639A (ja) 情報処理装置、情報処理方法およびプログラム
JP2010145219A (ja) 運動推定装置及びプログラム
WO2022118513A1 (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: 20805679

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: 20805679

Country of ref document: EP

Kind code of ref document: A1