WO2023162730A1 - 情報処理装置、情報処理方法及びプログラム - Google Patents

情報処理装置、情報処理方法及びプログラム Download PDF

Info

Publication number
WO2023162730A1
WO2023162730A1 PCT/JP2023/004645 JP2023004645W WO2023162730A1 WO 2023162730 A1 WO2023162730 A1 WO 2023162730A1 JP 2023004645 W JP2023004645 W JP 2023004645W WO 2023162730 A1 WO2023162730 A1 WO 2023162730A1
Authority
WO
WIPO (PCT)
Prior art keywords
image
point cloud
cloud data
control unit
imaging
Prior art date
Application number
PCT/JP2023/004645
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 WO2023162730A1 publication Critical patent/WO2023162730A1/ja

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/87—Combinations of systems using electromagnetic waves other than radio waves
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion

Definitions

  • the present disclosure relates to an information processing device, an information processing method, and a program.
  • An information processing apparatus includes an image acquisition unit, a point cloud data acquisition unit, and a control unit.
  • the image acquisition unit acquires an image from an imaging device.
  • the point cloud data acquisition unit acquires point cloud data representing a distance distribution from a distance measuring device.
  • the control unit obtains each of the first image and the second image based on the first image obtained from the image obtaining unit and the second image including the same subject as the first image. Acquire information indicating the correspondence between locations.
  • the control unit obtains two pieces of point cloud data obtained from the point cloud data obtaining unit, the first point cloud data in which the imaging range of the first image and the measurement range of the distance distribution at least partially overlap. and second point cloud data in which the imaging range of the second image and the measurement range of the distance distribution at least partially overlap, based on the information indicating the correspondence relationship.
  • An information processing method acquires a first image and a second image including the same subject as the first image, and two point cloud data representing a distance distribution.
  • first point cloud data in which the imaging range of the first image and the measurement range of the distance distribution at least partially overlap; and the imaging range of the second image and the measurement range of the distance distribution are at least partially overlapped. and acquiring information indicating the correspondence relationship between the positions of the first image and the second image based on the first image and the second image. and associating the first point cloud data with the second point cloud data based on the information indicating the correspondence relationship.
  • a program acquires a first image and a second image including the same subject as the first image, and two point cloud data indicating a distance distribution, The imaging range of the first image and the measurement range of the distance distribution at least partially overlap the first point cloud data, and the imaging range of the second image and the measurement range of the distance distribution at least partially overlap.
  • FIG. 1 is a configuration diagram showing a basic configuration of a posture estimation system including an information processing device according to an embodiment of the present disclosure.
  • FIG. 2 is a configuration diagram showing a more detailed example of the posture estimation system of FIG.
  • FIG. 3 is a block diagram showing a schematic configuration of the information processing apparatus of FIG. 1.
  • FIG. 4 is a flowchart showing processing executed by the information processing apparatus of FIG.
  • FIG. 5 is a diagram illustrating an example of an image acquired by the image acquiring unit in FIG. 3;
  • FIG. 6 is a diagram showing an example of a distance image based on point cloud data acquired by the point cloud data acquiring unit in FIG. 3 .
  • FIG. 7 is a diagram showing an example of image composition by associating the first image and the second image.
  • FIG. 1 is a configuration diagram showing a basic configuration of a posture estimation system including an information processing device according to an embodiment of the present disclosure.
  • FIG. 2 is a configuration diagram showing a more detailed example of the posture estimation system of FIG
  • FIG. 8 is a diagram illustrating a method of using homography transformation to associate the first image and the second image.
  • FIG. 9A is a diagram showing an example of the first point cloud data after dilation and distortion correction.
  • FIG. 9B is a diagram showing an example of the second point cloud data after dilation and distortion correction.
  • FIG. 10A is a diagram showing a first three-dimensional spatial point cloud created from the first point cloud data.
  • FIG. 10B is a diagram showing a second three-dimensional spatial point cloud created from the second point cloud data.
  • FIG. 11 is a diagram showing an example of an environment map generated using the information processing apparatus according to this embodiment.
  • FIG. 12 is a diagram showing an example of an environmental map generated using the same image and point cloud data as in FIG. 11 by a conventional method that can use feature points.
  • posture estimation means estimating the amount of rotation and the amount of movement from data of different viewpoints.
  • the point cloud data showing the distribution of distances to each measurement position obtained from the ranging device is composed of point clouds from different viewpoints. It may be difficult to make correspondence between data. As a result, sufficient posture estimation accuracy cannot be obtained, and the environment map accuracy cannot be improved in some cases.
  • the ICP Interactive Closest Point
  • the ICP Interactive Closest Point
  • each point forming one point group is searched for the closest point in the other point group, and this is taken as a corresponding point.
  • the method estimates, for each point thus determined and its corresponding points, a rigid transformation that minimizes the distance between the corresponding points.
  • Rigid body transformations are represented by rotations and translations.
  • Pose estimation can be done by estimating rigid transformations.
  • this method since the corresponding points cannot be determined correctly, there are cases where posture estimation cannot be performed or estimation errors increase.
  • each point of the two point cloud data of the distance measuring device using two images corresponding to the respective point clouds of the imaging device whose imaging range is the same as the measurement range of the distance measuring device. be done. That is, it is possible to extract feature points corresponding to each other between two images, extract points located at the same positions as the feature points on the images of the two point cloud data corresponding to each image, and set them as corresponding points. can. By transforming a set of a plurality of corresponding points obtained in this way into a point group in a three-dimensional space, posture estimation becomes possible. However, in this method, the points corresponding to the feature points on the image are often not included in the point cloud data obtained from the distance measuring device.
  • the ranging device is not suitable for edge and corner ranging. Therefore, the points that can be associated with the point cloud data may be limited. Therefore, sufficient corresponding points cannot be obtained, and the accuracy of posture estimation may be lowered.
  • Embodiments of the present disclosure improve pose estimation methods as described above by facilitating correspondence between point cloud data.
  • embodiments of the present disclosure will be described with reference to the drawings.
  • the figures used in the following description are schematic.
  • the dimensions, ratios, etc. on the drawings do not necessarily match the actual ones.
  • a posture estimation system 10 is mounted on a mobile object.
  • the posture estimation system 10 includes a distance measuring device 11, an imaging device 12, and an information processing device 13, as shown in FIG.
  • the relative positions and orientations of the distance measuring device 11 and the imaging device 12 are fixed.
  • the information processing device 13 may be configured to communicate with the distance measuring device 11 and imaging device 12 that are not mounted on a moving object but are mounted on a moving object.
  • the rangefinder 11 is a scanning rangefinder such as a laser radar.
  • a laser radar measures the distance to a subject by irradiating a measurement range with laser light and measuring the time it takes for the reflected laser light to return.
  • Laser radar is also called LiDAR (Light Detection And Ranging).
  • the distance measuring device 11 outputs point cloud data indicating the distance distribution of the measurement range.
  • the distance distribution is the distribution of distances from the distance measuring device 11 to each measurement position.
  • the point cloud data includes distance information detected by the distance measuring device 11 scanning the entire measurement range.
  • the point cloud data includes, for example, one frame of distance data from the distance measuring device 11 .
  • Each point of the point cloud data includes position information when the object to be measured is viewed two-dimensionally, and information on the distance from the distance measuring device 11 corresponding to the position information to the measurement position.
  • the two-dimensional position information corresponds to the direction from the distance measuring device 11 toward each measurement position in real space.
  • the imaging device 12 has an imaging optical system and an imaging element, and acquires an image of the imaging range.
  • the imaging range of the imaging device 12 at least partially overlaps the measurement range of the distance measuring device 11 .
  • the distance measuring device 11 and the imaging device 12 can be configured so that the coordinate systems for observing the target are substantially the same or close to each other.
  • the distance measuring device 11 and the imaging device 12 may be configured such that the optical axis ax of the optical system 14 for detecting or imaging a subject is aligned. In this case, the distance measuring device 11 and the imaging device 12 have the same optical axis and a fixed positional relationship. The upper position can be associated in advance.
  • the distance measuring device 11 and the imaging device 12 may be arranged so that the optical axes of the optical systems for measuring or imaging the subject are parallel and close to each other. In this case, the optical system 14 for matching the mutual optical axes may be omitted. Between the point cloud data and the image to be detected by the distance measuring device 11 and the imaging device 12, there is no parallax or the parallax is small. The point cloud data output from the distance measuring device 11 and the image output from the imaging device 12 can be superimposed so that the positions of the same frames correspond to each other.
  • the distance measuring device 11 and the imaging device 12 may be fixed with respect to the moving body so that their relative positions and orientations are fixed.
  • Mobile objects include, for example, automobiles, robots, humans, handcarts, wheelchairs, home electric appliances such as self-propelled vacuum cleaners, unmanned aircraft such as drones, toys with mobile functions, and articles attached to these including any moving object such as
  • the information processing device 13 is a computer.
  • the information processing device 13 may be any of various computers including general-purpose computers, workstations, and PCs (Personal Computers).
  • the information processing device 13 may be a dedicated computer that performs specific processing for constructing the environment map and estimating the self-location.
  • the information processing device 13 may read a dedicated program and data and execute each function of the information processing device 13 described below.
  • the posture estimation system 10 includes a detection device 21 that includes the functions of the ranging device 11 and the imaging device 12 and an information processing device 13 .
  • the detection device 21 includes an irradiation unit 22 , a reflection unit 23 , a control unit 24 , a first optical system 25 and a detection element 26 as components corresponding to the distance measurement device 11 .
  • the detection device 21 also includes a control unit 24 , a second optical system 27 , and an imaging device 28 as components corresponding to the imaging device 12 .
  • the detection device 21 further includes a switching section 32 .
  • the irradiation unit 22 radiates at least one electromagnetic wave of infrared rays, visible rays, ultraviolet rays, and radio waves. In one embodiment, the irradiation section 22 emits infrared rays.
  • the irradiating unit 22 irradiates the radiated electromagnetic wave toward the object ob, which is a subject, directly or indirectly via the reflecting unit 23 . In the embodiment illustrated in FIG. 2 , the irradiation unit 22 indirectly irradiates the radiated electromagnetic wave toward the object ob through the reflection unit 23 .
  • the object ob includes all objects located within the measurement range of the distance measuring device 11 .
  • the irradiation unit 22 radiates a beam-shaped electromagnetic wave with a narrow width, for example, 0.5°.
  • the irradiation unit 22 can emit electromagnetic waves in pulses.
  • the irradiation unit 22 includes LEDs (Light Emitting Diodes) and LDs (Laser Diodes). The irradiation unit 22 switches between electromagnetic wave radiation and stop based on the control of the control unit 24, which will be described later.
  • the reflecting unit 23 changes the irradiation position of the electromagnetic wave irradiated to the object ob by reflecting the electromagnetic wave emitted from the irradiation unit 22 while changing the direction.
  • the irradiation position is equal to the measurement position where the distance is measured by the distance measuring device 11 .
  • the reflecting unit 23 scans the measurement range including the object ob with the electromagnetic waves emitted from the irradiating unit 22 .
  • the reflecting unit 23 can scan the irradiation position of the electromagnetic waves in two-dimensional directions.
  • the reflecting unit 23 can perform regular scanning by sequentially shifting the irradiation position detected by the distance measuring device 11 in the vertical direction while scanning in the horizontal direction.
  • Reflector 23 may scan the measurement range in any other manner.
  • the reflecting section 23 is configured such that at least part of the irradiation area of the electromagnetic wave emitted from the irradiating section 22 and reflected is included in the detection range of the detecting element 26 . Therefore, at least a part of the electromagnetic waves irradiated to the object ob through the reflector 23 can be detected by the detection element 26 .
  • the reflecting section 23 includes, for example, a MEMS (Micro Electro Mechanical Systems) mirror, a polygon mirror, a galvanomirror, and the like.
  • MEMS Micro Electro Mechanical Systems
  • the reflecting section 23 changes the direction in which the electromagnetic waves are reflected under the control of the control section 24, which will be described later.
  • the reflection unit 23 may have an angle sensor such as an encoder, and may notify the control unit 24 of the angle detected by the angle sensor as direction information for reflecting the electromagnetic waves.
  • the controller 24 can calculate the irradiation position based on the direction information acquired from the reflector 23 .
  • the control unit 24 can calculate the irradiation position based on the drive signal input to the reflecting unit 23 to change the direction in which the electromagnetic waves are reflected.
  • the reflection unit 23 is configured such that the central axis of the direction in which the electromagnetic wave is scanned is the imaging device. It can be arranged substantially parallel and close to the twelve optical axes.
  • the control unit 24 includes one or more processors and memory.
  • the processor may include at least one of a general-purpose processor that loads a specific program and executes a specific function, and a dedicated processor that specializes in specific processing.
  • a dedicated processor may include an Application Specific Integrated Circuit (ASIC).
  • the processor may include a programmable logic device (PLD).
  • the PLD may include an FPGA (Field-Programmable Gate Array).
  • the control unit 24 may include at least one of SoC (System-on-a-Chip) and SiP (System In a Package) in which one or more processors cooperate.
  • the control unit 24 is configured to be able to control the reflection unit 23 and the switching unit 32 .
  • the control unit 24 can control the switching unit 32 so that the detecting element 26 can acquire the reflected electromagnetic wave according to the irradiation position and the irradiation time of the electromagnetic wave by the reflecting unit 23 .
  • the control unit 24 can acquire detection information from the detection element 26 and calculate the distance.
  • the control unit 24 can generate point cloud data from each measurement position and the distance at the measurement position.
  • the control unit 24 can acquire an image signal from the imaging device 28 .
  • the control unit 24 can output point cloud data and image signals to the information processing device 13 .
  • the first optical system 25 is irradiated from the irradiation unit 22 and reflected by the reflection unit 23, so that the reflected wave from the object ob of the electromagnetic wave irradiated toward the measurement range is detected by the detection element 26. proceed as follows. As will be described later, the optical axis of the first optical system 25 may be configured to match the optical axis of the second optical system 27 on the object ob side.
  • the detection element 26 includes an element capable of detecting electromagnetic waves emitted from the irradiation section 22 .
  • the detection element 26 includes a single element such as an APD (Avalanche PhotoDiode) and a PD (PhotoDiode).
  • Detector elements 26 may include element arrays such as APD arrays, PD arrays, ranging imaging arrays, and ranging image sensors.
  • the detection element 26 transmits detection information indicating detection of the reflected wave from the subject to the control section 24 as a signal.
  • the detection element 26 detects, for example, electromagnetic waves in the infrared band.
  • the detection element 26 only needs to be able to detect electromagnetic waves in a configuration that is a single element that constitutes the distance measuring sensor described above, and it is not necessary for the object ob to be imaged on the detection surface. Therefore, the detection element 26 does not have to be provided at the secondary image forming position, which is the image forming position by the first post-stage optical system 30, which will be described later. That is, in this configuration, if the detecting element 26 is at a position where electromagnetic waves from all angles of view can be incident on the detecting surface, the detecting element 26 travels in the first direction d1 by the switching unit 32, and then moves toward the first post-optical system. It may be placed anywhere on the path of the electromagnetic wave traveling through 30 .
  • the control unit 24 acquires the distance based on the electromagnetic waves detected by the detection element 26. Based on the detection information detected by the detection element 26, the control unit 24 acquires the distance of the irradiation position irradiated by the irradiation unit 22 by a ToF (Time-of-Flight) method as described below.
  • ToF Time-of-Flight
  • the control unit 24 By inputting an electromagnetic wave radiation signal to the irradiation unit 22, the control unit 24 causes the irradiation unit 22 to radiate pulsed electromagnetic waves.
  • the irradiation unit 22 irradiates an electromagnetic wave based on the input electromagnetic wave radiation signal.
  • the electromagnetic waves radiated by the irradiation unit 22 and reflected by the reflection unit 23 to irradiate an arbitrary irradiation area are reflected in the irradiation area.
  • the detection element 26 detects the electromagnetic waves reflected in the irradiation area, the detection element 26 notifies the control unit 24 of detection information.
  • the control unit 24 measures the time from the time when the irradiation unit 22 emits electromagnetic waves to the time when the detection information is acquired.
  • the control unit 24 multiplies the time by the speed of light and divides by 2 to calculate the distance to the irradiation position.
  • the control unit 24 calculates the irradiation position based on the direction information acquired from the reflection unit 23 or the drive signal output to the reflection unit 23 by itself, as described above. do.
  • the control unit 24 can calculate the irradiation position based on the position at which the electromagnetic wave reflected by the object ob is detected on the element array.
  • the irradiation position is the measurement position of the distance measuring device 11 as described above.
  • the control unit 24 calculates the distance to each irradiation position while changing the irradiation position, thereby creating point cloud data including information on a plurality of measurement positions and distances.
  • the second optical system 27 forms an image of the object ob in a range overlapping the measurement range of the distance measuring device 11 on the imaging surface of the imaging device 28 .
  • the imaging device 28 converts the image formed on the imaging surface into an electrical signal to generate an image of the imaging range including the object ob.
  • the imaging device 28 may include either a CCD image sensor (Charge-Coupled Device Image Sensor) or a CMOS image sensor (Complementary MOS Image Sensor).
  • the imaging device 28 outputs the generated image to the control unit 24.
  • the control unit 24 may perform arbitrary processing such as distortion correction, brightness adjustment, contrast adjustment, gamma correction, etc. on the image.
  • the control unit 24 may correct the distortion. If there is a deviation between the optical system that detects the point cloud data and the optical system that captures the image, the control unit 24 controls at least one of the point cloud data and the image so as to reduce the influence of the deviation. can be corrected. Such point cloud data and image correction may be performed by the information processing device 13 instead of the control unit 24 .
  • the first optical system 25 includes a pre-stage optical system 29 common to the second optical system 27 and a first post-stage optical system 30 positioned downstream of the switching section 32 .
  • the second optical system 27 includes a pre-stage optical system 29 that is common to the first optical system 25 and a second post-stage optical system 31 positioned after the switching section 32 .
  • the pre-stage optical system 29 includes, for example, at least one of a lens and a mirror, and forms an image of an object ob, which is a subject.
  • the switching unit 32 is provided at or near the primary imaging position, which is the imaging position of the image of the object ob, which is separated from the preceding optical system 29 by the preceding optical system 29, by the preceding optical system 29. It is good if there is
  • the switching unit 32 has an action surface as on which the electromagnetic wave that has passed through the pre-stage optical system 29 is incident.
  • the active surface as is composed of a plurality of pixels px arranged two-dimensionally.
  • the action surface as is a surface that causes an electromagnetic wave to have an action such as reflection or transmission in at least one of a first state and a second state described later.
  • the switching unit 32 can switch for each pixel px between a first state in which an electromagnetic wave incident on the action surface as travels in a first direction d1 and a second state in which it travels in a second direction d2.
  • a first state is a first reflection state in which an electromagnetic wave incident on the working surface as is reflected in a first direction d1.
  • the second state is a second reflection state in which the electromagnetic wave incident on the action surface as is reflected in the second direction d2.
  • the switching unit 32 includes a reflecting surface that reflects electromagnetic waves for each pixel px.
  • the switching unit 32 switches between the first reflection state and the second reflection state for each pixel px by changing the orientation of the reflection surface for each pixel px.
  • the switching unit 32 includes, for example, a DMD (Digital Micro mirror Device).
  • the DMD drives a minute reflecting surface that constitutes the working surface as so that the reflecting surface is tilted at a predetermined angle, for example +12° or ⁇ 12°, with respect to the working surface as for each pixel px. can be switched to The active surface as is parallel to the surface of the substrate on which the minute reflecting surface of the DMD is placed.
  • the switching unit 32 switches between the first state and the second state for each pixel px under the control of the control unit 24 .
  • the switching unit 32 can simultaneously switch some of the pixels px1 to the first state so that the electromagnetic waves incident on the pixels px1 travel in the first direction d1, and switch some of the pixels px2 to the first state.
  • the electromagnetic waves incident on the pixel px2 can be caused to travel in the second direction d2.
  • the switching unit 32 switches the same pixel px from the first state to the second state, thereby directing the electromagnetic waves incident on the pixel px in the first direction d1 and then in the second direction d2. can proceed.
  • the first post-stage optical system 30 is provided from the switching section 32 in the first direction d1.
  • the first post-stage optical system 30 includes, for example, at least one of a lens and a mirror.
  • the first post-stage optical system 30 causes the electromagnetic wave whose traveling direction is switched by the switching unit 32 to enter the detection element 26 .
  • the second post-stage optical system 31 is provided from the switching section 32 in the second direction d2.
  • the second post-stage optical system 31 includes, for example, at least one of a lens and a mirror.
  • the second post-stage optical system 31 forms an image of the target ob as an electromagnetic wave whose traveling direction is switched by the switching unit 32 on the imaging surface of the imaging device 28 .
  • the detection device 21 aligns the optical axis ax of the front optical system 29 with the optical axis of the first rear optical system 30 in the first state, It is possible to match the optical axis of the optical system 31 . Therefore, by switching the pixel px of the switching unit 32 between the first state and the second state, the detection device 21 detects the measurement position where the detection element 26 in the case of an element array detects the distance and the image pickup element. 28 can reduce the parallax shift with the image captured.
  • the control unit 24 can switch some of the pixels px in the switching unit 32 to the first state and switch some other pixels px to the second state as the reflecting unit 23 moves the irradiation position. Therefore, the detection device 21 can cause the detection element 26 to detect electromagnetic waves in some pixels px and simultaneously cause the imaging element 28 to detect images in another part of pixels px. As a result, the detection device 21 can substantially simultaneously acquire an image of a portion excluding the distance of the irradiation position and the vicinity of the irradiation position of the electromagnetic waves in the same field of view.
  • an optical system including the switching unit 32 is used in order to make the optical axis of the distance measuring device 11 for measuring the distance and the optical axis of the imaging device 12 match or approach each other.
  • the method for making the optical axes of the distance measuring device 11 and the imaging device 12 coincide or approach each other is not limited to this.
  • the optical system 14 in FIG. 1 can be configured using the difference in wavelength between the light detected by the distance measuring device 11 and the image captured by the imaging device 12 .
  • the optical system 14 can be configured to match the optical axis of the optical system of the distance measuring device 11 and the optical axis of the imaging device 12 on the object ob side using a dichroic mirror, a dichroic prism, or the like.
  • the information processing apparatus 13 includes a point cloud data acquisition unit 41, an image acquisition unit 42, a control unit 43, a storage unit 44, and an output unit 45, as shown in FIG.
  • the point cloud data acquisition unit 41 acquires point cloud data representing the distance distribution from the distance measuring device 11 .
  • the point cloud data acquisition unit 41 may include a communication module that communicates with the distance measuring device 11 .
  • the point cloud data acquisition unit 41 acquires the point cloud data from the control unit 24 forming part of the distance measuring device 11 .
  • the point cloud data includes at least two-dimensional position information indicating the measurement position when the measurement range is viewed as a secondary plane, and the measured value of the distance to the object ob positioned at that position.
  • the image acquisition unit 42 acquires images from the imaging device 12 .
  • the image acquisition unit 42 may include a communication module that communicates with the imaging device 12 .
  • the image acquisition section 42 acquires an image from the control section 24 forming part of the imaging device 12 .
  • the time when the distance measuring device 11 acquires the point cloud data and the time when the imaging device 12 acquires the image may be synchronized.
  • the control unit 43 like the control unit 24 of the detection device 21, includes one or more processors and memory. Like the control unit 24 of the detection device 21, the control unit 43 includes at least one of a general-purpose processor that loads a specific program and executes a specific function, and a dedicated processor that specializes in specific processing. OK.
  • the control unit 43 can acquire point cloud data and images in which the measurement range and the imaging range when viewed as a two-dimensional plane at least partially overlap.
  • the control unit 43 can acquire the point cloud data and the image that are measured and captured substantially at the same time as a set of data.
  • Substantially the same time includes the time when the distance measuring device 11 measures one frame of point cloud data and the time when the imaging device 12 captures an image.
  • the distance measurement device 11 and the imaging device 12 perform distance measurement and imaging based on the same timing signal.
  • the control unit 43 can sequentially acquire two sets of point cloud data and images.
  • the control unit 43 sets the first set of point cloud data and the image as the first point cloud data and the first image, and sets the second set of the point cloud data and the image as the second point cloud data and the second image. It can be an image.
  • the control unit 43 calculates information indicating the correspondence between the positions of the first image and the second image, and generates the first point cloud data and the second point cloud data based on the information indicating this correspondence. can be mapped with
  • the correspondence between the first point cloud data and the second point cloud data is performed by associating each point included in the first point cloud data with each point data included in the second point cloud data. means.
  • each point of the first point cloud data and the second point cloud data that are associated is the first image and It is not limited to the points located at the feature points of the second image.
  • the posture estimation system 10 is configured such that the control unit 43 calculates information indicating the correspondence between the positions of the first image and the second image. may be configured such that the information is calculated by an information processing device on a network such as a server or cloud, and the information calculated by the information processing device is acquired by the control unit 43 .
  • the control unit 43 can estimate the amount of rotation and the amount of movement of the distance measuring device 11 and the imaging device 12 based on the correspondence between the first point cloud data and the second point cloud data. That is, the control unit 43 can estimate the posture of the moving object.
  • the control unit 43 can construct a map of the surrounding environment and estimate the self-position within the environmental map from the sequentially acquired point cloud data and the estimated amount of rotation and amount of movement.
  • the storage unit 44 includes semiconductor memory and/or magnetic memory.
  • the storage unit 44 may function, for example, as a main memory device, an auxiliary memory device, or a cache memory.
  • the storage unit 44 stores arbitrary information used for the operation of the information processing device 13 .
  • the storage unit 44 may store system programs, application programs, management databases, and the like.
  • the storage unit 44 can temporarily store the point cloud data acquired by the point cloud data acquisition unit 41, the image acquired by the image acquisition unit 42, and information obtained by processing these data.
  • the storage unit 44 may sequentially accumulate and store information on the coordinates of the point cloud data located in the surrounding environment and the amount of rotation and amount of movement.
  • the storage unit 44 may store environment maps sequentially constructed by the control unit 43 .
  • the output unit 45 is configured to be able to output information from the information processing device 13 to the outside.
  • the output unit 45 includes one or more of a display, a communication module that transmits information to an external computer, a device that outputs information to a storage medium, and the like.
  • the output unit 45 may output the constructed environment map and the estimated self-location information to another system of the mobile body on which the information processing device 13 is mounted, via an electric line.
  • the detection device 21 and the information processing device 13 are separated.
  • the information processing device 13 may be incorporated into the same hardware as the detection device 21 of FIG.
  • the control unit 43 of the information processing device 13 may be shared with the control unit 24 of the detection device 21 .
  • Posture estimation processing executed by the control unit 43 will be described below with reference to the flowchart of FIG.
  • the information processing device 13 may be configured to read a program recorded on a non-temporary computer-readable medium and implement the processing performed by the control unit 43, which will be described below.
  • Non-transitory computer-readable media include, but are not limited to, magnetic storage media, optical storage media, magneto-optical storage media, and semiconductor storage media.
  • the control unit 43 acquires the first image and the first point cloud data via the image acquisition unit 42 and the point cloud data acquisition unit 41 (step S101).
  • the first image and the first point cloud data have the same or close viewpoints for imaging and measurement. Note that when the measurement range of the distance measuring device 11 and the imaging range of the imaging device 12 only partially overlap, the first image and the first point cloud data for which the following processes are performed are obtained from the overlapping portion. shall be The same applies to the second image and the second point cloud data.
  • FIG. 5 shows an example of an image acquired by the image acquisition unit 42.
  • FIG. FIG. 6 shows an example of displaying the point cloud data acquired by the point cloud data acquiring unit 41 as a two-dimensional distance image.
  • the distance to each measurement position within the field of view of the distance measuring device 11 expressed in two-dimensional space is represented by the color or brightness at each point indicating each measurement position.
  • the control unit 43 acquires a second image and second point cloud data via the image acquisition unit 42 and the point cloud data acquisition unit 41 (step S102).
  • the viewpoint for capturing and measuring the second image and the second point cloud data by rotating and moving the moving body on which the distance measuring device 11 and the imaging device 12 are mounted changes the first image and the first point cloud model. It is different from the imaged and measured viewpoint.
  • the imaging range of the second image and the imaging range of the first image deviate from each other, the imaging ranges partially overlap each other.
  • the measurement range of the second point cloud data and the measurement range of the first point cloud data deviate, the measurement ranges partially overlap each other.
  • the first image and the first point cloud data are captured and measured at a first time
  • the second image and the second point cloud data are captured at a second time following the first time. and may be measured.
  • the control unit 43 may sequentially acquire images and point cloud data captured and measured at the same time.
  • a set of first image and first point cloud data and a set of second image and second point cloud data are obtained, and rotated between a first time and a second time Estimate volume and displacement.
  • the amount of rotation and the amount of movement after the second time can also be sequentially estimated in the same manner. Note that the first image and the first point cloud data do not necessarily have to be captured and measured at the same time.
  • the capturing of the first image and the measurement of the first point cloud data are sequentially performed at different times. be able to. The same applies to the second image and the second point cloud data.
  • steps S103 to S105 and steps S106 to S108 are a series of processes.
  • Steps S103 to S105 are processes for the first image and the second image.
  • Steps S106 to S108 are processes for the first point cloud data and the second point cloud data. Steps S103 to S105 and steps S106 to S108 may be interchanged or executed in parallel.
  • step S102 if there is distortion in the acquired first image and second image, the control unit 43 performs correction to reduce the distortion (step S103).
  • the first image and the second image may have barrel distortion as shown in FIG. 5 due to the lens of the imaging device 12 or the like.
  • the control unit 43 can correct this so that the image becomes a rectangle.
  • a first image and a second image as illustrated in the upper part of FIG. 7 are obtained.
  • the control unit 43 extracts feature points of the first image and the second image (step S104).
  • various methods including, for example, SHIFT (Scale-Invariant Feature Transform), SURF (Speeded Up Robust Features), and ORB (Oriented FAST and Rotated Brief) can be used.
  • SHIFT Scale-Invariant Feature Transform
  • SURF Speeded Up Robust Features
  • ORB Oriented FAST and Rotated Brief
  • RANSAC Random SAmple Consensus
  • RANSAC Random SAmple Consensus
  • the control unit 43 uses the feature points to calculate information indicating the correspondence between the first image and the second image (step S105).
  • the control unit 43 can obtain the correspondence relationship for each pixel between the first image and the second image by overlapping the images by matching feature points using a panoramic image creation technique. For example, the control unit 43 can superimpose the feature points of the second image on the feature points of the first image by performing homography transformation on the second image.
  • the control unit 43 calculates a transformation matrix for homography transformation from the feature points of the first image and the feature points of the second image.
  • the transformation matrix of homography transformation is information indicating the correspondence relationship between the first image and the second image.
  • FIG. 7 is an image of an office in which desks, chairs, and the like are arranged.
  • FIG. 8 is a simple drawing showing an image of synthesis using homographic transformation of the first image and the second image.
  • the transformation matrix of the homography transformation has a minimum of two so as to minimize the error between the feature points when the feature points of the first image and the feature points of the second image are matched using the homography transformation. It may be estimated using multiplication.
  • the number of measurement points (corresponding to points in the point cloud data) that the distance measuring device 11 can acquire in one scan is smaller than the number of pixels of the image captured by the imaging device 12 . Therefore, the control unit 43 performs processing for converting the first point cloud data and the second point cloud data into data having the same number of pixels as the number of pixels of the image (step S106). The control unit 43 converts the number of vertical and horizontal points of the first point cloud data and the second point cloud data to match the number of vertical and horizontal pixels of the first image and the second image. do. Thereby, each pixel of the first image and the second image is associated with any one of the first point cloud data and the second point cloud data, respectively.
  • step S107 if the expanded first point cloud data and second point cloud data have distortion, the control unit 43 corrects the distortion. This distortion correction may be performed before step S106 or after step S108, which will be described later, instead of after step S106.
  • FIGS. 9A and 9B show an example of displaying the first point cloud data and the second point cloud data after expansion and distortion correction as distance images.
  • FIGS. 9A and 9B it is difficult to detect edges of a subject with point cloud data, unlike images.
  • point cloud data it is easy to acquire distance data of measurement points on a flat portion such as a wall.
  • the point cloud data of FIGS. 9A and 9B are shown as examples. 9A and 9B are obtained by measuring the inside of an office with desks, chairs, etc., similar to FIG. 7, but are obtained by measuring a scene different from the first image and the second image shown in FIG. It has become.
  • the control unit 43 calculates point groups in a three-dimensional space from the expanded and distortion-corrected first point group data and second point group data (step S108). That is, the control unit 43 converts point cloud data (u, v, d) in a two-dimensional space consisting of two-dimensional coordinates (u, v) indicating the position of each point of the point cloud data and distance d between each point to Convert to coordinates (x, y, z) of a point in 3D space.
  • the coordinates (x, y, z) in the three-dimensional space are the positions measured by the distance measuring device 11 in the three-dimensional space.
  • the three-dimensional coordinates (x, y, z) are defined, for example, with the position of the distance measuring device 11 as the origin, the direction of the optical system in which the distance measuring device 11 detects the object ob as the z axis, and the direction perpendicular to the z axis.
  • a coordinate system having two axes perpendicular to each other as the x-axis and the y-axis can be used.
  • the coordinate axes of this three-dimensional space are fixed with respect to the distance measuring device 11, and rotate and move as the distance measuring device 11 rotates and moves.
  • 10A and 10B show point clouds in a three-dimensional space calculated based on the first point cloud data corresponding to FIG. 9A and the second point cloud data corresponding to FIG. 9B, respectively, viewed from a specific direction. It is represented as a diagram. 10A and 10B are observations of overlapping measurement ranges from different viewpoints.
  • control unit 43 associates the first point cloud data with the second point cloud data using the information indicating the correspondence relationship between the first image and the second image calculated in step S105. to acquire the coordinates of the corresponding points in the three-dimensional space (step S109). Step S109 will be described below in two stages.
  • the control unit 43 generates two-dimensional first point cloud data and second two-dimensional point cloud data having the same point arrays as the pixel arrays of the first image and the second image as shown in FIGS. 9A and 9B. Each point is associated with the point cloud data. Based on the information indicating the correspondence calculated in step S105, the control unit 43 extracts points of the first point cloud data and points of the second point cloud data located at mutually corresponding positions (u, v). do. For example, in step S105, when the control unit 43 calculates the transformation matrix for homographically transforming the second image into the first image, the control unit 43 adds this Apply a transformation matrix.
  • the control unit 43 extracts the points of the first point cloud data whose positions (u, v) match those of the converted second point cloud data. If the points of the extracted first point cloud data and the points of the second point cloud data have a distance d that is substantially equal to each other in consideration of the error and the range of movable distances, the two points correspond to It can be determined that there are In this method, the correspondence between the points on the first point cloud data and the points on the second point cloud data is examined based on the information indicating the correspondence. Therefore, much more points can be associated than when point cloud data are associated using only the feature points of the image.
  • the control unit 43 assigns a Obtain the coordinates (x, y, z) of the corresponding point in three-dimensional space.
  • the coordinates (x, y, z) in the three-dimensional space corresponding to the points of the two-dimensional point cloud data are calculated in step S108.
  • These coordinates (x, y, z) are the coordinates of the points of the first point group data and the points of the second point group data corresponding to each other represented by the coordinates of the three-dimensional space.
  • the control unit 43 excludes unnecessary points from the set of corresponding points represented by the coordinates (x, y, z) in the three-dimensional space (step S110). For example, points whose distance is calculated as 0 are deleted. Also, points that are determined to be abnormal values may be deleted.
  • the control unit 43 estimates the posture of the posture estimation system 10 and the moving object equipped with the posture estimation system 10 using the coordinates of the corresponding points in the three-dimensional space (step S111).
  • Posture estimation can be performed by obtaining a rotation matrix and a translation vector so as to minimize the distance between corresponding points included in the first point cloud data and the second point cloud data.
  • q i (i 1, . 2, . . . , n).
  • n is the number of corresponding points of each point cloud data.
  • R be the desired rotation matrix and t the desired translation vector.
  • the rotation matrix R and translation vector t are obtained by the following formula (1).
  • control unit 43 can calculate the amount of rotation and the amount of movement of the moving body between the first time and the second time to estimate the posture.
  • control unit 43 can sequentially acquire the point cloud data measured by the distance measuring device 11 and the image captured by the imaging device 12 while the moving body sequentially changes its position and posture. Based on the estimated amount of rotation and movement of the moving object, the control unit 43 can reconstruct the information of the measured point cloud data in the global coordinate system and construct an environment map. The control unit 43 can estimate the self-position and orientation of the mobile object on the constructed environment map of the global coordinate system.
  • the first point cloud data and the second point cloud data are obtained using information indicating the correspondence relationship between the first image and the second image. Since the corresponding points with the data are extracted, many corresponding points can be extracted. This improves the accuracy of posture estimation. Furthermore, when constructing an environment map using this attitude estimation system 10, the accuracy of attitude estimation is improved, so accumulation of errors in attitude estimation can be reduced, and a more accurate environment map can be constructed. can.
  • the present inventor prepares point cloud data and images obtained by rotating a moving body equipped with the posture estimation system 10 by 10 degrees in a horizontal plane by 10 degrees in total 360 degrees, and prepares point cloud data and images. was created. Further, as a comparative example, the present inventor extracted feature points from the same point cloud data and image by ORB from the image, and used these feature points to associate the point cloud data, thereby creating an environment map. made.
  • FIG. 11 is an environmental map constructed by the method of the present disclosure.
  • FIG. 12 is an environmental map constructed by the method of the comparative example. In either case, the angle ⁇ is the error in the amount of rotation accumulated as a result of a 360 degree rotation.
  • the inventor also performed pose estimation on data obtained by removing outliers by RANSAC from feature points by ORB.
  • the method of the present disclosure also showed significant accuracy improvements over this method.
  • the information processing device 13 of the present disclosure performs correspondence between the first point cloud data and the second point cloud data acquired from different viewpoints by the distance measuring device 11 for more points. It can be done easily. This improves the accuracy of posture estimation of the moving body. Furthermore, the improved accuracy of pose estimation improves the accuracy of environment map construction and self-location estimation.
  • attitude estimation system 11 ranging device 12 imaging device 13 information processing device 14 optical system 21 detection device 22 irradiation unit 23 reflection unit 24 control unit 25 first optical system 26 detection element 27 second optical system 28 imaging element 29 front stage Optical system 30 First post-stage optical system 31 Second post-stage optical system 32 Switching unit 41 Point group data acquisition unit 42 Image acquisition unit 43 Control unit 44 Storage unit ax Optical axis d1 First direction d2 Second direction ob Object (subject)

Landscapes

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

Abstract

本開示の情報処理装置は、画像取得部と、点群データ取得部と、制御部とを備える。画像取得部は、撮像装置からの画像を取得する。点群データ取得部は、測距装置から距離分布を示す点群データを取得する。制御部は、画像取得部から取得した第１の画像、及び、第１の画像と同一の被写体を含む第２の画像に基づき、第１の画像及び第２の画像の各位置の対応関係を示す情報を取得する。制御部は、点群データ取得部から取得した２つの点群データであって、第１の画像の撮像範囲と距離分布の測定範囲が少なくとも部分的に重なる第１の点群データと、第２の画像の撮像範囲と距離分布の測定範囲が少なくとも部分的に重なる第２の点群データとを、対応関係を示す情報に基づいて対応付ける。

Description

情報処理装置、情報処理方法及びプログラム 関連出願の相互参照
　本出願は、２０２２年２月２４日に出願された日本国特許出願２０２２－０２７２６９号の優先権を主張するものであり、この先の出願の開示全体をここに参照のために取り込む。
　本開示は、情報処理装置、情報処理方法、及びプログラムに関する。
　環境内を動き回る移動ロボットに搭載される技術であって、カメラ画像を用いて環境地図を構築するとともに、地図内での自己位置を推定する技術が提案されている（例えば、特許文献１参照）。
特開２０２０－１１７７７９号公報
　本開示の一実施形態に係る情報処理装置は、画像取得部と、点群データ取得部と、制御部とを備える。前記画像取得部は、撮像装置からの画像を取得する。前記点群データ取得部は、測距装置から距離分布を示す点群データを取得する。前記制御部は、前記画像取得部から取得した第１の画像、及び、前記第１の画像と同一の被写体を含む第２の画像に基づき、前記第１の画像及び前記第２の画像の各位置の対応関係を示す情報を取得する。前記制御部は、前記点群データ取得部から取得した２つの点群データであって、前記第１の画像の撮像範囲と前記距離分布の測定範囲が少なくとも部分的に重なる第１の点群データと、前記第２の画像の撮像範囲と前記距離分布の測定範囲が少なくとも部分的に重なる第２の点群データとを、前記対応関係を示す情報に基づいて対応付ける。
　本開示の一実施形態に係る情報処理方法は、第１の画像及び前記第１の画像と同一の被写体を含む第２の画像を取得することと、距離分布を示す２つの点群データであって、前記第１の画像の撮像範囲と前記距離分布の測定範囲が少なくとも部分的に重なる第１の点群データと、前記第２の画像の撮像範囲と前記距離分布の測定範囲が少なくとも部分的に重なる第２の点群データとを取得することと、前記第１の画像及び前記第２の画像に基づき、前記第１の画像及び前記第２の画像の各位置の対応関係を示す情報を取得することと、該対応関係を示す情報に基づいて、前記第１の点群データと、前記第２の点群データとの対応付けをすることとを含む。
　本開示の一実施形態に係るプログラムは、第１の画像及び前記第１の画像と同一の被写体を含む第２の画像を取得することと、距離分布を示す２つの点群データであって、前記第１の画像の撮像範囲と前記距離分布の測定範囲が少なくとも部分的に重なる第１の点群データと、前記第２の画像の撮像範囲と前記距離分布の測定範囲が少なくとも部分的に重なる第２の点群データとを取得することと、前記第１の画像及び前記第２の画像に基づき、前記第１の画像及び前記第２の画像の各位置の対応関係を示す情報を取得することと、該対応関係を示す情報に基づいて、前記第１の点群データと、前記第２の点群データとの対応付けをすることとを含む処理をコンピュータに実行させる。
図１は、本開示の一実施形態に係る情報処理装置を含む姿勢推定システムの基本的な構成を示す構成図である。 図２は、図１の姿勢推定システムのより詳細な一例を示す構成図である。 図３は、図１の情報処理装置の概略構成を示すブロック図である。 図４は、図１の情報処理装置が実行する処理を示すフローチャートである。 図５は、図３の画像取得部が取得する画像の一例を示す図である。 図６は、図３の点群データ取得部が取得する点群データに基づく距離画像の一例を示す図である。 図７は、第１の画像と第２の画像との対応付けによる画像合成の一例を示す図である。 図８は、第１の画像と第２の画像との対応付けにホモグラフィ変換を用いる方法を説明する図である。 図９Ａは、拡張及び歪み補正後の第１の点群データの一例を示す図である。 図９Ｂは、拡張及び歪み補正後の第２の点群データの一例を示す図である。 図１０Ａは、第１の点群データから作成された第１の３次元空間点群を示す図である。 図１０Ｂは、第２の点群データから作成された第２の３次元空間点群を示す図である。 図１１は、本実施形態に係る情報処理装置を用いて生成した環境地図の一例を示す図である。 図１２は、特徴点を用い得る従来手法により図１１と同じ画像及び点群データを用いて生成した環境地図の一例を示す図である。
　環境地図の構築及び自己位置推定は、移動体の姿勢推定をしながら環境の検出を行うことにより実行される。ここで、姿勢推定とは、異なる視点のデータから、回転量及び移動量を推定することを意味する。姿勢推定を行うためには、各視点のデータ間の対応付けをする必要がある。しかしながら、レーザレーダ等の測距装置を用いて環境地図の構築及び自己位置推定を行う場合、測距装置から得られる各測定位置までの距離の分布を示す点群データは、異なる視点の点群データ間で対応付けをすることが難しい場合がある。その結果、十分な姿勢推定の精度が得られず、環境地図の精度も向上できない場合がある。
　例えば、従来、２つの３次元の点群の位置合わせを行う手法としてＩＣＰ（Interactive Closest Point）アルゴリズムが知られている。この手法は、一方の点群を構成する各点に対して、他方の点群における最も近接する点を探索して、これを対応点とする。さらに、この手法は、このようにして決定した各点とその対応点に対して、対応点間の距離を最小化するような剛体変換を推定する。剛体変換は、回転及び併進で表される。剛体変換を推定することにより姿勢推定をすることができる。しかし、この方法では、対応点を正しく決定することができないため、姿勢推定ができない場合又は推定の誤差が大きくなる場合がある。
　一方、測距装置の２つの点群データの各点を、測距装置の測定範囲と同じ範囲を撮像範囲とする撮像装置のそれぞれの点群に対応する２つの画像を用いて対応付けることも考えられる。すなわち、２つの画像間で互いに対応する特徴点を抽出し、それぞれの画像に対応する２つの点群データの画像上の特徴点と同じ位置に位置する点を抽出し、対応点とすることができる。このようにして求めた複数の対応点の組を３次元空間の点群に変換すれば、姿勢推定が可能になる。しかし、この方法では、画像上の特徴点に対応する点が、測距装置から得られる点群データに含まれないことが多い。画像上の特徴点は、画像のエッジ及びコーナーから検出されるが、測距装置はエッジ及びコーナーの測距には向いていないからである。このため、点群データで対応付けができる点は限られることがある。そのため、十分な対応点が得られず、姿勢推定の推定精度が低くなることがある。
　本開示の実施形態は、点群データ間の対応付けを容易にすることで、上述のような姿勢推定の方法を改善する。以下、本開示の実施形態について、図面を参照して説明する。以下の説明で用いられる図は模式的なものである。図面上の寸法及び比率等は現実のものとは必ずしも一致していない。
（姿勢推定システムの基本構成）
　本開示の一実施形態に係る姿勢推定システム１０は、移動体に搭載される。姿勢推定システム１０は、図１に示すように、測距装置１１、撮像装置１２及び情報処理装置１３を含む。測距装置１１と撮像装置１２とは、相対的な位置及び向きが固定されている。他の実施形態において、情報処理装置１３は、移動体に搭載されず、移動体に搭載された測距装置１１及び撮像装置１２と通信可能に構成されてよい。
　測距装置１１は、レーザレーダ等の走査型の測距装置である。レーザレーダは、測定範囲にレーザ光を照射し、反射されたレーザ光が戻ってくるまでの時間を計測することにより被写体までの距離を計測する。レーザレーダは、ＬｉＤＡＲ（Ｌｉｇｈｔ　Ｄｅｔｅｃｔｉｏｎ　Ａｎｄ　Ｒａｎｇｉｎｇ）とも称される。測距装置１１は、測定範囲の距離分布を示す点群データを出力する。距離分布とは、測距装置１１から各測定位置までの距離の分布である。点群データには、測距装置１１が測定範囲全体を走査して検出した距離情報が含まれる。点群データには、例えば、測距装置１１の１フレームの距離データが含まれる。
　点群データの各点は、測定対象を２次元的に見たときの位置情報と、当該位置情報に対応する測距装置１１から測定位置までの距離の情報を含む。２次元の位置情報は、測距装置１１から実空間上の各測定位置に向かう方向に対応する。
　撮像装置１２は、撮像光学系及び撮像素子を有し、撮像範囲の画像を取得する。撮像装置１２の撮像範囲は、測距装置１１の測定範囲と少なくとも部分的に重複する。
　測距装置１１と撮像装置１２とは、対象を観察する座標系が実質的に一致又は近接するように構成することができる。測距装置１１と撮像装置１２とは、被写体を検出又は撮像する光学系１４の光軸ａｘが、一致するように構成されてよい。この場合、測距装置１１と撮像装置１２とは、光軸が一致し位置関係が固定されているため、同一フレーム内において、画像上の画素の位置と点群データの各点の２次元平面上の位置とは、予め対応付けができる。
　測距装置１１と撮像装置１２とは、被写体を測定又は撮像する光学系の光軸が、互いに平行に近接して配置されてもよい。この場合、互いの光軸を一致させるための光学系１４は無くてもよい。測距装置１１と撮像装置１２とでは、検出する点群データ及び画像間に視差が無いか、又は、視差が小さい。測距装置１１の出力する点群データと撮像装置１２の出力する画像とは、同一フレーム同士で位置が対応するように重ね合わせることができる。
　測距装置１１及び撮像装置１２は、互いの相対的な位置及び向きが固定されるように、移動体に対して固定されてよい。移動体は、例えば、自動車、ロボット、人間、手押し車、車いす、自走式の掃除機等の家電機器、ドローン等の無人航空機、及び、移動機能を有する玩具、並びに、これらに装着される物品等の任意の移動体を含む。
　情報処理装置１３は、コンピュータである。情報処理装置１３は、汎用コンピュータ、ワークステーション、及び、ＰＣ（Ｐｅｒｓｏｎａｌ　Ｃｏｍｐｕｔｅｒ）等を含む種々のコンピュータの何れかであってよい。情報処理装置１３は、環境地図構築及び自己位置推定のための特定の処理を行う専用のコンピュータであってよい。情報処理装置１３は、専用のプログラム及びデータを読み込んで以下に説明する情報処理装置１３の各機能を実行してよい。
（姿勢推定システムの具体例）
　姿勢推定システム１０のより具体的な一例を、図２を参照して説明する。姿勢推定システム１０は、測距装置１１及び撮像装置１２の機能を包含する検出装置２１と、情報処理装置１３とを含む。検出装置２１は、測距装置１１に対応する構成要素として、照射部２２、反射部２３、制御部２４、第１の光学系２５及び検出素子２６を含む。また、検出装置２１は、撮像装置１２に対応する構成要素として、制御部２４、第２の光学系２７及び撮像素子２８を含む。検出装置２１は、さらに、切替部３２を含む。
（測距装置の構成）
　照射部２２は、赤外線、可視光線、紫外線、及び電波の少なくともいずれかの電磁波を放射する。一実施形態において、照射部２２は、赤外線を放射する。照射部２２は、放射する電磁波を、被写体である対象ｏｂに向けて、直接又は反射部２３を介して間接的に、照射する。図２に図示する実施形態において、照射部２２は、放射する電磁波を、対象ｏｂに向けて、反射部２３を介して間接的に照射する。対象ｏｂは、測距装置１１の測定範囲に位置する全ての物体を含む。
　一実施形態においては、照射部２２は、幅の細い、例えば０．５°のビーム状の電磁波を放射する。また、一実施形態において、照射部２２は電磁波をパルス状に放射可能である。例えば、照射部２２は、ＬＥＤ（Ｌｉｇｈｔ　Ｅｍｉｔｔｉｎｇ　Ｄｉｏｄｅ）及びＬＤ（Ｌａｓｅｒ　Ｄｉｏｄｅ）などを含む。照射部２２は、後述する制御部２４の制御に基づいて、電磁波の放射及び停止を切替える。
　反射部２３は、照射部２２から放射された電磁波を、向きを変えながら反射することにより、対象ｏｂに照射される電磁波の照射位置を変更する。照射位置は、測距装置１１により距離を測定される測定位置に等しい。反射部２３は、照射部２２から放射される電磁波により、対象ｏｂを含む測定範囲を走査する。
　反射部２３は、電磁波の照射位置を２次元方向に走査することができる。反射部２３は、測距装置１１が検出を行う照射位置を、水平方向に走査しながら順次垂直方向にずらしていく規則的な走査を行うことができる。反射部２３は、他の任意の方法で、測定範囲を走査してよい。
　反射部２３は、照射部２２から放射されて反射した電磁波の照射領域の少なくとも一部が、検出素子２６の検出範囲に含まれるように構成されている。したがって、反射部２３を介して対象ｏｂに照射される電磁波の少なくとも一部は、検出素子２６において検出され得る。
　反射部２３は、例えば、ＭＥＭＳ（Ｍｉｃｒｏ　Ｅｌｅｃｔｒｏ　Ｍｅｃｈａｎｉｃａｌ　Ｓｙｓｔｅｍｓ）ミラー、ポリゴンミラー、及びガルバノミラーなどを含む。
　反射部２３は、後述する制御部２４の制御に基づいて、電磁波を反射する向きを変える。また、反射部２３は、例えばエンコーダなどの角度センサを有してもよく、角度センサが検出する角度を、電磁波を反射する方向情報として、制御部２４に通知してもよい。このような構成において、制御部２４は、反射部２３から取得する方向情報に基づいて、照射位置を算出し得る。また、制御部２４は、反射部２３に電磁波を反射する向きを変えさせるために入力する駆動信号に基づいて照射位置を算出し得る。
　制御部２４が、反射部２３から取得する方向情報、又は、反射部２３に入力する駆動信号に基づいて照射位置を算出する場合、反射部２３は、電磁波を走査する方向の中心軸が撮像装置１２の光軸に実質的に平行且つ近接して配置することができる。
　制御部２４は、１以上のプロセッサ及びメモリを含む。プロセッサは、特定のプログラムを読み込ませて特定の機能を実行する汎用のプロセッサ、及び特定の処理に特化した専用のプロセッサの少なくともいずれかを含んでよい。専用のプロセッサは、特定用途向けＩＣ（ＡＳＩＣ；Ａｐｐｌｉｃａｔｉｏｎ　Ｓｐｅｃｉｆｉｃ　Ｉｎｔｅｇｒａｔｅｄ　Ｃｉｒｃｕｉｔ）を含んでよい。プロセッサは、プログラマブルロジックデバイス（ＰＬＤ；Ｐｒｏｇｒａｍｍａｂｌｅ　Ｌｏｇｉｃ　Ｄｅｖｉｃｅ）を含んでよい。ＰＬＤは、ＦＰＧＡ（Ｆｉｅｌｄ－Ｐｒｏｇｒａｍｍａｂｌｅ　Ｇａｔｅ　Ａｒｒａｙ）を含んでよい。制御部２４は、１つ又は複数のプロセッサが協働するＳｏＣ（Ｓｙｓｔｅｍ－ｏｎ－ａ－Ｃｈｉｐ）、及びＳｉＰ（Ｓｙｓｔｅｍ　Ｉｎ　ａ　Ｐａｃｋａｇｅ）の少なくともいずれかを含んでもよい。
　制御部２４は、反射部２３及び切替部３２を制御可能に構成される。制御部２４は、反射部２３による電磁波の照射位置及び照射時刻に応じて、検出素子２６が反射された電磁波を取得できるように切替部３２を制御することができる。制御部２４は、後述するように、検出素子２６から検出情報を取得し、距離を算出することができる。制御部２４は、各測定位置と当該測定位置における距離から点群データを生成することができる。また、制御部２４は、撮像素子２８から画像信号を取得することができる。制御部２４は、点群データ及び画像信号を情報処理装置１３に出力することができる。
　第１の光学系２５は、照射部２２から照射され且つ反射部２３により反射されることにより、測定範囲に向けて照射された電磁波の対象ｏｂからの反射波が、検出素子２６により検出されるように進行させる。後述するように、第１の光学系２５の光軸は、第２の光学系２７の光軸と、対象ｏｂ側において一致するように構成されてよい。
　検出素子２６は、照射部２２から出射した電磁波を検出可能な素子を含む。例えば、検出素子２６は、ＡＰＤ（Ａｖａｌａｎｃｈｅ　ＰｈｏｔｏＤｉｏｄｅ）、及びＰＤ（ＰｈｏｔｏＤｉｏｄｅ）などの単一の素子を含む。検出素子２６は、ＡＰＤアレイ、ＰＤアレイ、測距イメージングアレイ、及び測距イメージセンサなどの素子アレイを含むものであってもよい。一実施形態において、検出素子２６は、被写体からの反射波を検出したことを示す検出情報を信号として制御部２４に送信する。検出素子２６は、例えば、赤外線の帯域の電磁波を検出する。
　なお、検出素子２６は、上述した測距センサを構成する単一の素子である構成において、電磁波を検出できればよく、対象ｏｂが検出面において結像される必要はない。それゆえ、検出素子２６は、後述する第１の後段光学系３０による結像位置である二次結像位置に設けられなくてもよい。すなわち、この構成において、検出素子２６は、すべての画角からの電磁波が検出面上に入射可能な位置であれば、切替部３２により第１の方向ｄ１に進行した後に第１の後段光学系３０を経由して進行する電磁波の経路上のどこに配置されてもよい。
　制御部２４は、検出素子２６が検出した電磁波に基づいて、距離を取得する。制御部２４は、検出素子２６が検出する検出情報に基づいて、以下に説明するように、ＴｏＦ（Ｔｉｍｅ－ｏｆ－Ｆｌｉｇｈｔ）方式により、照射部２２により照射される照射位置の距離を取得する。
　制御部２４は、照射部２２に電磁波放射信号を入力することにより、照射部２２にパルス状の電磁波を放射させる。照射部２２は、入力された当該電磁波放射信号に基づいて電磁波を照射する。照射部２２が放射し且つ反射部２３が反射して任意の照射領域に照射された電磁波は、当該照射領域において反射する。そして、検出素子２６は、当該照射領域において反射された電磁波を検出するとき、検出情報を制御部２４に通知する。
　制御部２４は、照射部２２に電磁波を放射させた時刻から、検出情報を取得した時刻までの時間を計測する。制御部２４は、当該時間に、光速を乗算し、且つ２で除算することにより、照射位置までの距離を算出する。検出素子２６が単一の素子である場合、制御部２４は、上述のように、反射部２３から取得する方向情報、又は自身が反射部２３に出力する駆動信号に基づいて、照射位置を算出する。検出素子２６が素子のアレイを含む場合、制御部２４は、対象ｏｂにより反射された電磁波が素子アレイ上で検出される位置に基づいて、照射位置を算出することができる。前述のように照射位置は、測距装置１１の測定位置である。制御部２４は、照射位置を変えながら、各照射位置までの距離を算出することにより複数の測定位置と距離との情報を含む点群データを作成する。
（撮像装置の構成）
　第２の光学系２７は、測距装置１１の測定範囲と重複する範囲の対象ｏｂの像を撮像素子２８の撮像面上に結像させる。
　撮像素子２８は、撮像面において結像した像を電気信号に変換して、対象ｏｂを含む撮像範囲の画像を生成する。撮像素子２８は、ＣＣＤイメージセンサ（Ｃｈａｒｇｅ－Ｃｏｕｐｌｅｄ　Ｄｅｖｉｃｅ　Ｉｍａｇｅ　Ｓｅｎｓｏｒ）及びＣＭＯＳイメージセンサ（Ｃｏｍｐｌｅｍｅｎｔａｒｙ　ＭＯＳ　Ｉｍａｇｅ　Ｓｅｎｓｏｒ）の何れかを含んでよい。
　撮像素子２８は、生成した画像を制御部２４に出力する。制御部２４は、画像について、歪み補正、明度調整、コントラスト調整、ガンマ補正等の任意の処理を行ってよい。
　制御部２４は、点群データ及び画像の少なくとも何れかが歪みを含む場合、歪みを補正してよい。制御部２４は、点群データを検出する光学系と、画像を撮像する光学系との間にずれがある場合、当該ずれによる影響を低減するように、点群データ及び画像の少なくとも何れかを補正してよい。かかる、点群データ及び画像の補正は、制御部２４ではなく情報処理装置１３で行ってもよい。
（光学系と切替部の構成）
　第１の光学系２５は、第２の光学系２７と共通の前段光学系２９と、切替部３２の後段に位置する第１の後段光学系３０とを含む。第２の光学系２７は、第１の光学系２５と共通の前段光学系２９と、切替部３２の後段に位置する第２の後段光学系３１を含む。前段光学系２９は、例えば、レンズ及びミラーの少なくとも一方を含み、被写体となる対象ｏｂの像を結像させる。
　切替部３２は、前段光学系２９から所定の距離をおいて離れた対象ｏｂの像の、前段光学系２９による結像位置である一次結像位置、又は当該一次結像位置近傍に、設けられていればよい。切替部３２は、前段光学系２９を通過した電磁波が入射する作用面ａｓを有している。作用面ａｓは、２次元状に沿って並ぶ複数の画素ｐｘによって構成されている。作用面ａｓは、後述する第１の状態及び第２の状態の少なくともいずれかにおいて、電磁波に、例えば、反射及び透過などの作用を生じさせる面である。
　切替部３２は、作用面ａｓに入射する電磁波を、第１の方向ｄ１に進行させる第１の状態と、第２の方向ｄ２に進行させる第２の状態とに、画素ｐｘ毎に切替可能である。第１の状態は、作用面ａｓに入射する電磁波を、第１の方向ｄ１に反射する第１の反射状態である。また、第２の状態は、作用面ａｓに入射する電磁波を、第２の方向ｄ２に反射する第２の反射状態である。
　切替部３２は、さらに具体的には、画素ｐｘ毎に電磁波を反射する反射面を含んでいる。切替部３２は、画素ｐｘ毎の反射面の向きを変更することにより、第１の反射状態及び第２の反射状態を画素ｐｘ毎に切替える。一実施形態において、切替部３２は、例えばＤＭＤ（Ｄｉｇｉｔａｌ　Ｍｉｃｒｏ　ｍｉｒｒｏｒ　Ｄｅｖｉｃｅ：デジタルマイクロミラーデバイス）を含む。ＤＭＤは、作用面ａｓを構成する微小な反射面を駆動することにより、画素ｐｘ毎に当該反射面を作用面ａｓに対して所定の角度、例えば＋１２°及び－１２°のいずれかの傾斜状態に切替可能である。なお、作用面ａｓは、ＤＭＤにおける微小な反射面を載置する基板の板面に平行である。
　切替部３２は、制御部２４の制御に基づいて、第１の状態及び第２の状態を、画素ｐｘ毎に切替える。例えば、切替部３２は、同時に、一部の画素ｐｘ１を第１の状態に切替えることにより当該画素ｐｘ１に入射する電磁波を第１の方向ｄ１に進行させ得、別の一部の画素ｐｘ２を第２の状態に切替えることにより当該画素ｐｘ２に入射する電磁波を第２の方向ｄ２に進行させ得る。また、切替部３２は、同一の画素ｐｘを第１の状態から第２の状態に切替えることにより、当該画素ｐｘに入射する電磁波を第１の方向ｄ１の次に第２の方向ｄ２に向けて進行させ得る。
　図２に示すように、第１の後段光学系３０は、切替部３２から第１の方向ｄ１に設けられている。第１の後段光学系３０は、例えば、レンズ及びミラーの少なくとも一方を含む。第１の後段光学系３０は、切替部３２において進行方向を切替えられた電磁波を、検出素子２６に入射させる。
　第２の後段光学系３１は、切替部３２から第２の方向ｄ２に設けられている。第２の後段光学系３１は、例えば、レンズ及びミラーの少なくとも一方を含む。第２の後段光学系３１は、切替部３２において進行方向を切替えられた電磁波としての対象ｏｂの画像を撮像素子２８の撮像面上に結像させる。
　以上のような構成により、検出装置２１は、前段光学系２９の光軸ａｘを、第１の状態において第１の後段光学系３０の光軸に合わせ、かつ第２の状態において第２の後段光学系３１の光軸に合わせることが可能となる。したがって、検出装置２１は、切替部３２の画素ｐｘを第１の状態及び第２の状態のいずれかに切替えることにより、素子アレイである場合の検出素子２６が距離を検出する測定位置と撮像素子２８が撮像する画像との視差のずれを低減し得る。
　制御部２４は、反射部２３が照射位置を移動させるに従って、切替部３２における一部の画素ｐｘを第１の状態に切替え、且つ別の一部の画素ｐｘを第２の状態に切替え得る。したがって、検出装置２１は、一部の画素ｐｘにおいて検出素子２６に電磁波を検出させながら、同時に別の一部の画素ｐｘにおいて撮像素子２８に画像を検出させ得る。これにより、検出装置２１は、同じ視野内の照射位置の距離と電磁波の照射位置の近傍を除いた部分の画像とを実質的に同時に取得することができる。
　なお、図２の構成例では、測距装置１１の距離を測定する光軸と、撮像装置１２の光軸とを一致又は近接させるために、切替部３２を含む光学系を用いた。しかし、測距装置１１と撮像装置１２との光軸を一致又は近接させる方法は、これに限られない。例えば、図１の光学系１４は、測距装置１１の検出する光と撮像装置１２の撮像する画像との波長の差異を利用して構成することができる。例えば、光学系１４は、ダイクロイックミラー又はダイクロイックプリズム等を用いて、測距装置１１の光学系の光軸と撮像装置１２の光軸とを対象ｏｂ側で一致させる構成とすることができる。
（情報処理装置）
　一実施形態に係る情報処理装置１３は、図３に示すように、点群データ取得部４１、画像取得部４２、制御部４３、記憶部４４及び出力部４５を含む。
　点群データ取得部４１は、測距装置１１から距離分布を示す点群データを取得する。点群データ取得部４１は、測距装置１１と通信する通信モジュールを含んでよい。図２の検出装置２１の場合、点群データ取得部４１は、測距装置１１の一部を構成する制御部２４から点群データを取得する。点群データは、少なくとも、測定範囲を２次平面として見たときの測定位置を示す２次元の位置情報と、当該位置に位置する対象ｏｂまでの距離の測定値を含む。
　画像取得部４２は、撮像装置１２から画像を取得する。画像取得部４２は、撮像装置１２と通信する通信モジュールを含んでよい。図２の検出装置２１の場合、画像取得部４２は、撮像装置１２の一部を構成する制御部２４から画像を取得する。測距装置１１が点群データを取得する時刻と、撮像装置１２が画像を取得する時刻とは同期していてよい。
　制御部４３は、検出装置２１の制御部２４と同様に、１以上のプロセッサ及びメモリを含む。制御部４３は、検出装置２１の制御部２４と同様に、特定のプログラムを読み込ませて特定の機能を実行する汎用のプロセッサ、及び特定の処理に特化した専用のプロセッサの少なくともいずれかを含んでよい。
　制御部４３は、２次元平面として見たときの測定範囲と撮像範囲とが少なくとも部分的に重なる、点群データ及び画像を取得することができる。制御部４３は、実質的に同時刻に測定及び撮像された点群データ及び画像を、一組のデータとして取得することができる。実質的に同時刻には、測距装置１１が１フレームの点群データを測定する時刻と、撮像装置１２が画像を撮像する時刻とが重なることを含む。また、実質的に同時刻には、測距装置１１と撮像装置１２とが、同じタイミング信号に基づいて、測距及び撮像を行うことを含む。
　制御部４３は、二組の点群データ及び画像を順次取得することができる。制御部４３は、第１組の点群データ及び画像を、第１の点群データ及び第１の画像とし、第２組の点群データ及び画像を、第２の点群データ及び第２の画像とすることができる。制御部４３は、第１の画像及び第２の画像の各位置の対応関係を示す情報を算出し、この対応関係を示す情報に基づいて、第１の点群データと第２の点群データのとの対応付けをすることができる。第１の点群データと第２の点群データのとの対応付けは、第１の点群データに含まれる各点と第２の点群データとに含まれる各点のデータを対応付けることを意味する。第１の画像及び第２の画像の各位置の対応関係を示す情報を用いることにより、対応付けされる第１の点群データ及び第２の点群データの各点は、第１の画像及び第２の画像の特徴点に位置する点に限定されない。尚、本実施形態に係る姿勢推定システム１０は、第１の画像及び第２の画像の各位置の対応関係を示す情報を制御部４３が算出するように構成されているが、姿勢推定システム１０は、当該情報をサーバやクラウドなどのネットワーク上の情報処理装置に算出させ、その情報処理装置が算出した情報を制御部４３が取得するように構成されても良い。
　制御部４３は、第１の点群データと第２の点群データとの対応付けに基づいて、測距装置１１及び撮像装置１２の回転量及び移動量を推定することができる。すなわち、制御部４３は、移動体の姿勢推定をすることができる。制御部４３は、順次取得される点群データ及び、推定された回転量及び移動量の情報から、周辺環境の地図を構築するとともに、当該環境地図内における自己位置を推定することができる。
　記憶部４４は、半導体メモリ及び／又は磁気メモリを含む。記憶部４４は、例えば主記憶装置、補助記憶装置、又はキャッシュメモリとして機能してもよい。記憶部４４は、情報処理装置１３の動作に用いられる任意の情報を記憶する。例えば、記憶部４４は、システムプログラム、アプリケーションプログラム、管理データベース等を記憶してもよい。記憶部４４は、点群データ取得部４１で取得した点群データ及び画像取得部４２で取得した画像、並びに、これらから加工して得られた情報を、一時的に保存することができる。
　記憶部４４は、周辺環境に位置する点群データの座標、並びに、回転量及び移動量の情報を順次蓄積して記憶してよい。記憶部４４は、制御部４３により順次構築される環境地図を記憶してよい。
　出力部４５は、情報処理装置１３から外部に情報を出力可能に構成される。出力部４５は、ディスプレイ、外部のコンピュータに情報を送信する通信モジュール、記憶媒体に情報を出力する装置等の何れか一つ以上を含む。出力部４５は、情報処理装置１３を搭載する移動体の他のシステムに対して、構築した環境地図及び推定した自己位置の情報を、電気回線を通じて出力してよい。
　なお、図２では、検出装置２１と情報処理装置１３を別体とした。しかし、情報処理装置１３は、図２の検出装置２１と同一のハードウェアに組み込まれてもよい。情報処理装置１３の制御部４３は、検出装置２１の制御部２４と共通化されていてもよい。
（姿勢推定処理）
　以下に、制御部４３が実行する姿勢推定処理を、図４のフローチャートを用いて説明する。情報処理装置１３は、以下に説明する制御部４３が行う処理を、非一時的なコンピュータ可読媒体に記録されたプログラムを読み込んで実装するように構成されてよい。非一時的なコンピュータ可読媒体は、磁気記憶媒体、光学記憶媒体、光磁気記憶媒体、半導体記憶媒体を含むがこれらに限られない。
　制御部４３は、画像取得部４２及び点群データ取得部４１を介して、第１の画像及び第１の点群データを取得する（ステップＳ１０１）。第１の画像と第１の点群データとは、撮像及び測定する視点が一致する又は近接する。なお、測距装置１１の測定範囲と撮像装置１２の撮像範囲とが、一部においてのみ重なっている場合は、重なり部分から以下の処理を行う第１の画像及び第１の点群データが取得されるものとする。第２の画像及び第２の点群データについても同様とする。
　図５は、画像取得部４２で取得される画像の一例を示す。図６は、点群データ取得部４１で取得される点群データを、２次元の距離画像として表示した一例を示す。距離画像では、２次元空間で表された測距装置１１の視野内の各測定位置までの距離が、各測定位置を示す各点における色又は輝度により表現される。
　ステップＳ１０１の後、制御部４３は、画像取得部４２及び点群データ取得部４１を介して、第２の画像及び第２の点群データを取得する（ステップＳ１０２）。測距装置１１及び撮像装置１２を搭載する移動体の回転及び移動により、第２の画像及び第２の点群データを撮像及び測定する視点は、第１の画像及び第１の点群モデルを撮像及び測定した視点とは異なる。これにより、第２の画像の撮像範囲と、第１の画像の撮像範囲とはずれるが、互いの撮像範囲は一部が重なっている。また、第２の点群データの測定範囲と、第１の点群データの測定範囲とはずれるが、互いの測定範囲は一部が重なっている。
　第１の画像と第１の点群データとは、第１の時刻に撮像及び測定され、第２の画像と第２の点群データとは、第１の時刻に続く第２の時刻に撮像及び測定されてよい。制御部４３は、第２の時刻以降も順次同一の時刻に撮像及び測定された画像と点群データとを取得してよい。以下では、第１の画像及び第１の点群データの組と、第２の画像及び第２の点群データの組を取得して、第１の時刻と第２の時刻との間の回転量及び移動量を推定する。第２の時刻以降の回転量及び移動量も、同様にして順次推定することができる。なお、第１の画像と第１の点群データとは、必ずしも同一の時刻に撮像及び測定されなくてもよい。第１の画像を撮像する撮像範囲及び第１の点群データを取得する測定範囲に変化が無ければ、第１の画像の撮像と第１の点群データの測定とは、異なる時間に順次行うことができる。第２の画像及び第２の点群データについても同様である。
　以下のステップＳ１０３からＳ１０５と、ステップＳ１０６からＳ１０８とは、それぞれ一連の処理である。ステップＳ１０３からＳ１０５は、第１の画像及び第２の画像に対する処理である。ステップＳ１０６からＳ１０８は、第１の点群データ及び第２の点群データに対する処理である。ステップＳ１０３からＳ１０５と、ステップＳ１０６からＳ１０８とは、入れ替えてもよく、並行して実行されてもよい。
　ステップＳ１０２の後、制御部４３は、取得した第１の画像及び第２の画像に歪みがある場合、歪みを減少させる補正を行う（ステップＳ１０３）。例えば、第１の画像及び第２の画像は、撮像装置１２の有するレンズ等に起因して、図５に示すように樽型の歪を有することがある。制御部４３は、これを画像が長方形となるように補正することができる。歪の補正により、例えば、図７の上段に例示されるような第１の画像と第２の画像とが取得される。
　次に、制御部４３は、第１の画像及び第２の画像の特徴点を抽出する（ステップＳ１０４）。特徴点を抽出する方法としては、例えば、ＳＨＩＦＴ（Ｓｃａｌｅ－Ｉｎｖａｒｉａｎｔ　Ｆｅａｔｕｒｅ　Ｔｒａｎｓｆｏｒｍ）、ＳＵＲＦ（Ｓｐｅｅｄｅｄ　Ｕｐ　Ｒｏｂｕｓｔ　Ｆｅａｔｕｒｅｓ）、及び、ＯＲＢ（Ｏｒｉｅｎｔｅｄ　ＦＡＳＴ　ａｎｄ　Ｒｏｔａｔｅｄ　ＢＲＩＥＦ）を含む種々の手法を用いることができる。また、特徴点を抽出するとき、ノイズなどによる外れ値の影響を除外する方法としてＲＡＮＳＡＣ（ＲＡＮｄｏｍ　ＳＡｍｐｌｅ　Ｃｏｎｓｅｎｓｕｓ）が知られている。
　制御部４３は、特徴点を用いて、第１の画像と第２の画像との対応関係を示す情報を算出する（ステップＳ１０５）。制御部４３は、パノラマ画像作成技術を用いて、特徴点をマッチングすることにより画像どうしを重ね合わせることで、第１の画像と第２の画像との画素毎の対応関係を得ることができる。例えば、制御部４３は、第２の画像に対してホモグラフィ変換を行うことにより、第２の画像の特徴点を第１の画像の特徴点に重ね合わせることができる。制御部４３は、第１の画像の特徴点と第２の画像の特徴点とから、ホモグラフィ変換の変換行列を算出する。この場合、ホモグラフィ変換の変換行列は、第１の画像と第２の画像との対応関係を示す情報である。
　第１の画像と第２の画像との対応関係を示す情報を用いることにより、図７に示すように、第１の画像と第２の画像とを合成することができる。図７は、机及び椅子等が配置されたオフィス内の画像である。図８は、第１の画像と第２の画像とのホモグラフィ変換を用いた合成のイメージを簡単な図面で示したものである。図８の例では、長方形の第２の画像を台形に変形させることにより、第１の画像の座標系に射影して特徴点を一致させる。ホモグラフィ変換の変換行列は、第１の画像の特徴点と第２の画像の特徴点とをホモグラフィ変換を用いてマッチングさせたときに、特徴点間の誤差を最小化するように最小二乗法を用いて推定してよい。
　次に、ステップＳ１０６からステップＳ１０８について説明する。
　測距装置１１が、１回の走査で取得できる測定点（点群データの各点に相当）の数は、撮像装置１２により撮像される画像の画素数よりも少ない。このため、制御部４３は、第１の点群データ及び第２の点群データを画像の画素数と同じ画素数をもつデータに変換する処理を行う（ステップＳ１０６）。制御部４３は、第１の点群データ及び第２の点群データの縦及び横の点の数を、第１の画像及び第２の画像の縦及び横の画素数と一致するように変換する。これにより、第１の画像及び第２の画像の各画素は、それぞれ第１の点群データ及び第２の点群データの何れかの点に対応付けられる。
　次に、制御部４３は、拡張された第１の点群データ及び第２の点群データが歪を有する場合、歪みの補正をする（ステップＳ１０７）。この歪みの補正は、ステップＳ１０６の後ではなく、ステップＳ１０６の前又は後述するステップＳ１０８の後に行ってもよい。
　図９Ａ及び図９Ｂは、拡張及び歪み補正後の第１の点群データ及び第２の点群データを距離画像として表示した一例を示す。図９Ａ及び図９Ｂからも明らかなように、点群データでは、画像のように被写体のエッジを検出することは難しい。その一方、点群データでは、壁などの平らな部分の測定点の距離データを取得し易い。なお、図９Ａ及び図９Ｂの点群データは例示として示したものである。図９Ａ及び図９Ｂは、図７と同様に机及び椅子等があるオフィス内を測定したものであるが、図７に示した第１の画像及び第２の画像とは異なる場面を測定したものとなっている。
　次に、制御部４３は、拡張及び歪み補正された第１の点群データ及び第２の点群データから、それぞれ３次元の空間の点群を算出する（ステップＳ１０８）。すなわち、制御部４３は、点群データの各点の位置を示す２次元の座標（ｕ，ｖ）と各点の距離ｄよりなる２次元空間の点群データ（ｕ，ｖ，ｄ）を、３次元空間の点の座標（ｘ，ｙ，ｚ）に変換する。３次元空間の座標（ｘ，ｙ，ｚ）は、３次元空間における測距装置１１による測定位置である。ここで、３次元の座標（ｘ，ｙ，ｚ）は、例えば、測距装置１１の位置を原点とし、測距装置１１が対象ｏｂを検出する光学系の方向をｚ軸、ｚ軸に直交し且つ互いに直交する２軸をｘ軸及びｙ軸とする座標系とすることができる。この３次元空間の座標軸は、測距装置１１に対して固定され、測距装置１１が回転及び移動するとともに回転及び移動する。
　図１０Ａ及び図１０Ｂは、それぞれ図９Ａに対応する第１の点群データ及び図９Ｂに対応する第２の点群データに基づいて算出された３次元空間の点群を、特定の方向から見た図として表したものである。図１０Ａと図１０Ｂとは、重複する測定範囲を異なる視点から観察したものとなっている。
　次に、制御部４３は、ステップＳ１０５で算出した第１の画像と第２の画像との対応関係を示す情報を用いて、第１の点群データと第２の点群データとの対応付けを行い、対応点の３次元空間での座標を取得する（ステップＳ１０９）。ステップＳ１０９について、以下に２段階に分けて説明する。
　まず、制御部４３は、図９Ａ及び９Ｂに示したような、第１の画像及び第２の画像の画素配列と同じ点の配列を有する、２次元の第１の点群データ及び第２の点群データに対して、各点の対応付けを行う。制御部４３は、ステップＳ１０５で算出した対応関係を示す情報に基づいて、互いに対応する位置（ｕ，ｖ）に位置する第１の点群データの点と第２の点群データの点を抽出する。例えば、ステップＳ１０５で制御部４３が第２の画像を第１の画像にホモグラフィ変換する変換行列を算出した場合、制御部４３は、２次元の第２の点群データの各点に、この変換行列を適用する。制御部４３は、変換された第２の点群データの点と位置（ｕ，ｖ）が一致する第１の点群データの点を抽出する。抽出された第１の点群データの点と第２の点群データの点が、誤差及び移動可能な距離の範囲を考慮して互いに略等しい距離ｄを有する場合、２つの点は対応していると判断できる。この方法では、対応関係を示す情報に基づいて、第１の点群データ上の点と第２の点群データ上の点全体について対応関係が調べられる。このため、画像の特徴点のみを用いて点群データの対応付けを行う場合よりも、はるかに多くの点の対応付けが可能になる。
　次に、制御部４３は、２次元空間で対応付けられた第１の点群データの点（ｕ，ｖ，ｄ）及び第２の点群データの点（ｕ，ｖ，ｄ）のそれぞれに対応する点の３次元空間の座標（ｘ，ｙ，ｚ）を取得する。２次元の点群データの点に対応する３次元空間の座標（ｘ，ｙ，ｚ）は、ステップＳ１０８で算出されている。これらの座標（ｘ，ｙ，ｚ）は、３次元空間の座標で表された互いに対応する第１の点群データの点と第２の点群データの点の座標である。
　制御部４３は、３次元空間の座標（ｘ，ｙ，ｚ）で表された対応点の組から、不要な点を除外する（ステップＳ１１０）。例えば、距離が０と算出される点は、削除される。また、異常値と判断される点は削除されてよい。
　制御部４３は、対応点の３次元空間の座標を用いて、姿勢推定システム１０及びこれを搭載する移動体の姿勢を推定する（ステップＳ１１１）。姿勢推定は、第１の点群データ及び第２の点群データに含まれる各対応点間の距離を最小化するように、回転行列及び並進ベクトルを求めることによって行うことができる。例えば、第１の点群データの対応点の座標をｐｉ（ｉ＝１，２，・・・，ｎ）とし、第２の点群データの対応点の座標をｑｉ（ｉ＝１，２，・・・，ｎ）とする。ここで、ｎは各点群データの対応点の数である。また、Ｒを目的の回転行列、ｔを目的の並進ベクトルとする。回転行列Ｒ及び並進ベクトルｔは、次の数式（１）により求められる。
Figure JPOXMLDOC01-appb-M000001
　以上のように、制御部４３は、第１の時刻と第２の時刻との間の移動体の回転量及び移動量を算出して姿勢推定をすることができる。
　また、制御部４３は、移動体が順次位置及び姿勢を変えながら測距装置１１が測定した点群データ、及び、撮像装置１２が撮像した画像を順次取得することができる。制御部４３は、推定した移動体の回転量及び移動量に基づいて、測定される点群データの情報をグローバル座標系で再構成し、環境地図を構築することができる。制御部４３は、構築したグローバル座標系の環境地図における移動体の自己位置と姿勢とを推定することができる。
　本開示の情報処理装置１３を用いた姿勢推定システム１０によれば、第１の画像と第２の画像との対応関係を示す情報を用いて、第１の点群データと第２の点群データとの対応点を抽出したので、多くの対応点を抽出することができる。これにより、姿勢推定の精度が向上する。さらに、この姿勢推定システム１０を用いて、環境地図を構築した場合、姿勢推定の精度が向上しているので、姿勢推定の誤差の蓄積を低減することができるので、より正確な環境地図が構築できる。
（比較実験）
　本発明者は、姿勢推定システム１０が搭載された移動体を、水平面内で１０度ずつ合計３６０度回転させて測定及び撮像した点群データ及び画像を用意して、本開示の方法により環境地図の作成を行った。また、本発明者は、比較例として、同じ点群データ及び画像について、画像からＯＲＢによる特徴点を抽出して、この特徴点を用いて点群データの対応付けを行うことにより、環境地図を作製した。図１１は、本開示の方法により構築した環境地図である。図１２は、比較例の方法により構築した環境地図である。何れにおいても、角度θは、３６０度周回した結果蓄積された回転量の誤差である。
　図１１及び図１２から、本開示の方法は、比較例の方法よりも誤差の蓄積が小さく精度が向上することが分かる。
　本発明者は、ＯＲＢによる特徴点にＲＡＮＳＡＣによる外れ値の除外を行ったデータについても、姿勢推定を行った。本開示の方法は、この方法に対しても顕著な精度の改善が見られた。
　以上説明したように、本開示の情報処理装置１３は、測距装置１１により異なる視点で取得された第１の点群データ及び第２の点群データ間の対応付けを、より多くの点について容易に行うことができる。これにより、移動体の姿勢推定の精度が向上する。さらに、姿勢推定の精度が向上することにより、環境地図の構築及び自己位置推定の精度が向上する。
　本開示に係る実施形態について、諸図面及び実施例に基づき説明してきたが、当業者であれば本開示に基づき種々の変形又は修正を行うことが容易であることに注意されたい。従って、これらの変形又は修正は本開示の範囲に含まれることに留意されたい。例えば、各構成部又は各ステップなどに含まれる機能などは論理的に矛盾しないように再配置可能であり、複数の構成部又はステップなどを１つに組み合わせたり、或いは分割したりすることが可能である。本開示に係る実施形態は装置が備えるプロセッサにより実行される方法、プログラム、又はプログラムを記録した記憶媒体としても実現し得るものである。本開示の範囲にはこれらも包含されるものと理解されたい。
　１０　　姿勢推定システム
　１１　　測距装置
　１２　　撮像装置
　１３　　情報処理装置
　１４　　光学系
　２１　　検出装置
　２２　　照射部
　２３　　反射部
　２４　　制御部
　２５　　第１の光学系
　２６　　検出素子
　２７　　第２の光学系
　２８　　撮像素子
　２９　　前段光学系
　３０　　第１の後段光学系
　３１　　第２の後段光学系
　３２　　切替部
　４１　　点群データ取得部
　４２　　画像取得部
　４３　　制御部
　４４　　記憶部
　ａｘ　　光軸
　ｄ１　　第１の方向
　ｄ２　　第２の方向
　ｏｂ　　対象（被写体）

Claims (8)

  1. 　撮像装置から画像を取得する画像取得部と、
    　測距装置から距離分布を示す点群データを取得する点群データ取得部と、
    　前記画像取得部から取得した第１の画像、及び、前記第１の画像と同一の被写体を含む第２の画像に基づき、前記第１の画像及び前記第２の画像の各位置の対応関係を示す情報を取得し、前記点群データ取得部から取得した２つの点群データであって、前記第１の画像の撮像範囲と前記距離分布の測定範囲が少なくとも部分的に重なる第１の点群データと、前記第２の画像の撮像範囲と前記距離分布の測定範囲が少なくとも部分的に重なる第２の点群データとを、前記対応関係を示す情報に基づいて対応付ける制御部と
    を備える情報処理装置。
  2. 　前記制御部は、前記第１の点群データと前記第２の点群データとの対応付けに基づいて、前記撮像装置又は前記測距装置の回転量及び移動量を推定する、請求項１に記載の情報処理装置。
  3. 　前記撮像装置の画像を撮像する光学系と、前記測距装置の距離分布を測定する光学系とは、光軸が一致する、請求項１又は２の何れか一項に記載の情報処理装置。
  4. 　前記撮像装置と前記測距装置とは、前記第１の画像の撮像と前記第１の点群データ取得のための前記距離分布の測定とを同時刻に行い、前記第２の画像の撮像と前記第２の点群データ取得のための前記距離分布の測定とを同時刻に行うように構成される、請求項１から３の何れか一項に記載の情報処理装置。
  5. 　前記制御部は、前記第１の画像及び前記第２の画像に含まれる特徴点のマッチングを行うことにより、前記対応関係を示す情報を算出する請求項１から４の何れか一項に記載の情報処理装置。
  6. 　前記制御部は、ホモグラフィ変換を用いて前記特徴点のマッチングを行う、請求項５に記載の情報処理装置。
  7. 　第１の画像及び前記第１の画像と同一の被写体を含む第２の画像を取得することと、
    　距離分布を示す２つの点群データであって、前記第１の画像の撮像範囲と前記距離分布の測定範囲が少なくとも部分的に重なる第１の点群データと、前記第２の画像の撮像範囲と前記距離分布の測定範囲が少なくとも部分的に重なる第２の点群データとを取得することと、
    　前記第１の画像及び前記第２の画像に基づき、前記第１の画像及び前記第２の画像の各位置の対応関係を示す情報を取得することと、
    　該対応関係を示す情報に基づいて、前記第１の点群データと、前記第２の点群データとの対応付けをすることと
    を含む情報処理方法。
  8. 　第１の画像及び前記第１の画像と同一の被写体を含む第２の画像を取得することと、
    　距離分布を示す２つの点群データであって、前記第１の画像の撮像範囲と前記距離分布の測定範囲が少なくとも部分的に重なる第１の点群データと、前記第２の画像の撮像範囲と前記距離分布の測定範囲が少なくとも部分的に重なる第２の点群データとを取得することと、
    　前記第１の画像及び前記第２の画像に基づき、前記第１の画像及び前記第２の画像の各位置の対応関係を示す情報を取得することと、
    　該対応関係を示す情報に基づいて、前記第１の点群データと、前記第２の点群データとの対応付けをすることと
    を含む処理をコンピュータに実行させるためのプログラム。
PCT/JP2023/004645 2022-02-24 2023-02-10 情報処理装置、情報処理方法及びプログラム WO2023162730A1 (ja)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2022-027269 2022-02-24
JP2022027269 2022-02-24

Publications (1)

Publication Number Publication Date
WO2023162730A1 true WO2023162730A1 (ja) 2023-08-31

Family

ID=87765751

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2023/004645 WO2023162730A1 (ja) 2022-02-24 2023-02-10 情報処理装置、情報処理方法及びプログラム

Country Status (1)

Country Link
WO (1) WO2023162730A1 (ja)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117646828A (zh) * 2024-01-29 2024-03-05 中国市政工程西南设计研究总院有限公司 一种用于检测顶管接口相对位移和渗漏水的装置及方法

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2014173990A (ja) * 2013-03-08 2014-09-22 Topcon Corp 計測装置
US20200174130A1 (en) * 2017-08-04 2020-06-04 Bayerische Motoren Werke Aktiengesellschaft Method, Apparatus and Computer Program for a Vehicle
WO2021135157A1 (zh) * 2019-12-31 2021-07-08 山东大学 岩体结构探测及危石探测系统及方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2014173990A (ja) * 2013-03-08 2014-09-22 Topcon Corp 計測装置
US20200174130A1 (en) * 2017-08-04 2020-06-04 Bayerische Motoren Werke Aktiengesellschaft Method, Apparatus and Computer Program for a Vehicle
WO2021135157A1 (zh) * 2019-12-31 2021-07-08 山东大学 岩体结构探测及危石探测系统及方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117646828A (zh) * 2024-01-29 2024-03-05 中国市政工程西南设计研究总院有限公司 一种用于检测顶管接口相对位移和渗漏水的装置及方法
CN117646828B (zh) * 2024-01-29 2024-04-05 中国市政工程西南设计研究总院有限公司 一种用于检测顶管接口相对位移和渗漏水的装置及方法

Similar Documents

Publication Publication Date Title
JP5580164B2 (ja) 光学情報処理装置、光学情報処理方法、光学情報処理システム、光学情報処理プログラム
EP3438776B1 (en) Method, apparatus and computer program for a vehicle
US8446492B2 (en) Image capturing device, method of searching for occlusion region, and program
JP5465128B2 (ja) 点群位置データ処理装置、点群位置データ処理システム、点群位置データ処理方法、および点群位置データ処理プログラム
JP5620200B2 (ja) 点群位置データ処理装置、点群位置データ処理方法、点群位置データ処理システム、および点群位置データ処理プログラム
Draelos et al. The Kinect up close: Adaptations for short-range imaging
Varga et al. Super-sensor for 360-degree environment perception: Point cloud segmentation using image features
US20070247612A1 (en) System and method for improving lidar data fidelity using pixel-aligned lidar/electro-optic data
JP2015017992A (ja) 環境を光学的に走査および測定する方法ならびにデバイス
US11977167B2 (en) Efficient algorithm for projecting world points to a rolling shutter image
JP2006252473A (ja) 障害物検出装置、キャリブレーション装置、キャリブレーション方法およびキャリブレーションプログラム
JP2014534428A (ja) 空間コード化スライド画像に対するパターンのアライメント方法およびシステム
Martel et al. An active approach to solving the stereo matching problem using event-based sensors
JP6526955B2 (ja) センサ情報統合方法、及びその装置
WO2023162730A1 (ja) 情報処理装置、情報処理方法及びプログラム
JP2018155664A (ja) 撮像システム、撮像制御方法、画像処理装置および画像処理プログラム
Son et al. A lightweight and cost-effective 3D omnidirectional depth sensor based on laser triangulation
JP2008249431A (ja) ３次元画像補正方法及びその装置
Bileschi Fully automatic calibration of lidar and video streams from a vehicle
US10254402B2 (en) Stereo range with lidar correction
Hillemann et al. UCalMiCeL–Unified intrinsic and extrinsic calibration of a multi-camera-system and a laserscanner
Chen et al. Geometric calibration of a multi-layer LiDAR system and image sensors using plane-based implicit laser parameters for textured 3-D depth reconstruction
EP3255455A1 (en) Single pulse lidar correction to stereo imaging
JP7206855B2 (ja) 三次元位置検出装置、三次元位置検出システム、及び三次元位置検出方法
CN112419427A (zh) 用于提高飞行时间相机精度的方法

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

Country of ref document: EP

Kind code of ref document: A1