WO2018140701A1 - Laser scanner with real-time, online ego-motion estimation - Google Patents

Laser scanner with real-time, online ego-motion estimation Download PDF

Info

Publication number
WO2018140701A1
WO2018140701A1 PCT/US2018/015403 US2018015403W WO2018140701A1 WO 2018140701 A1 WO2018140701 A1 WO 2018140701A1 US 2018015403 W US2018015403 W US 2018015403W WO 2018140701 A1 WO2018140701 A1 WO 2018140701A1
Authority
WO
WIPO (PCT)
Prior art keywords
data
imu
estimated position
map
laser
Prior art date
Application number
PCT/US2018/015403
Other languages
French (fr)
Inventor
Ji Zhang
Kevin Joseph DOWLING
Sanjiv Singh
Original Assignee
Kaarta, Inc.
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
Priority claimed from PCT/US2017/021120 external-priority patent/WO2017155970A1/en
Priority claimed from PCT/US2017/055938 external-priority patent/WO2018071416A1/en
Priority to JP2019540661A priority Critical patent/JP7141403B2/en
Priority to EP18745179.4A priority patent/EP3574285A4/en
Application filed by Kaarta, Inc. filed Critical Kaarta, Inc.
Priority to EP18824609.4A priority patent/EP3646058A4/en
Priority to PCT/US2018/040269 priority patent/WO2019006289A1/en
Priority to PCT/US2018/042346 priority patent/WO2019018315A1/en
Priority to EP18834521.9A priority patent/EP3656138A4/en
Publication of WO2018140701A1 publication Critical patent/WO2018140701A1/en
Priority to US16/520,503 priority patent/US20190346271A1/en
Priority to US16/733,787 priority patent/US11573325B2/en
Priority to US16/745,775 priority patent/US10989542B2/en
Priority to US17/202,602 priority patent/US11506500B2/en
Priority to US17/964,307 priority patent/US20230288209A1/en

Links

Classifications

    • GPHYSICS
    • G01MEASURING; TESTING
    • G01SRADIO 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/00Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88Lidar systems specially adapted for specific applications
    • G01S17/89Lidar systems specially adapted for specific applications for mapping or imaging
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C11/00Photogrammetry or videogrammetry, e.g. stereogrammetry; Photographic surveying
    • G01C11/02Picture taking arrangements specially adapted for photogrammetry or photographic surveying, e.g. controlling overlapping of pictures
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1656Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with passive imaging devices, e.g. cameras
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01SRADIO 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/00Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06Systems determining position data of a target
    • G01S17/42Simultaneous measurement of distance and other co-ordinates
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01SRADIO 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/00Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01SRADIO 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
    • G01S7/00Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/4808Evaluating distance, position or velocity data
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01SRADIO 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
    • G01S7/00Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/481Constructional features, e.g. arrangements of optical elements
    • G01S7/4811Constructional features, e.g. arrangements of optical elements common to transmitter and receiver
    • G01S7/4813Housing arrangements

Definitions

  • PCT Application No. PCT/US2017/055938 claims priority to, and is a continuation-in-part of, PCT Application No. PCT/US2017/021120 (Atty. Dckt. No. KRTA- 0005 -WO) entitled "LASER SCANNER WITH REAL-TIME, ONLINE EGO-MOTION ESTIMATION," filed on March 7, 2017.
  • This application likewise claims priority to PCT Application No. PCT/US2017/021120.
  • PCT Application No. PCT/US2017/055938 further claims priority to U.S. Provisional No. 62/406,910 (Atty. Dckt. No. KRTA-0002-P02), entitled "LASER SCANNER WITH REAL-TIME, ONLINE EGO-MOTION
  • An autonomous moving device may require information regarding the terrain in which it operates. Such a device may either rely on a pre-defined map presenting the terrain and any obstacle that may be found therein. Alternatively, the device may have the capabilities to map its terrain while either stationary or in motion comprising a computer-based mapping system with one or more sensors to provide real-time data.
  • the mobile, computer-based mapping system may estimate changes in its position over time (an odometer) and/or generate a three- dimensional map representation, such as a point cloud, of a three-dimensional space.
  • an IMU may be coupled one or more cameras with, so that scale constraints may be provided from IMU accelerations.
  • a monocular camera may be tightly or loosely coupled to an IMU by means of a Kalman filter.
  • Other mapping systems may use optimization methods to solve for the motion of the mobile system.
  • mapping systems may include the use of laser scanners for motion estimation.
  • a difficulty of the use of such data may arise from the scanning rate of the laser. While the system is moving, laser points unlike a fixed position laser scanner are impacted by the relative movement of the scanner. Therefore the impact of this movement may be a factor of the laser points arriving arrive at the system at different times. Consequently, when the scanning rate is slow with respect to the motion of the mapping system, scan distortions may be present due to external motion of the laser.
  • the motion effect can be compensated by a laser itself but the compensation may require an independent motion model to provide the required corrections.
  • the motion may be modeled as a constant velocity or as a Gaussian process.
  • an IMU may provide the motion model.
  • Such a method matches spatio-temporal patches formed by laser point clouds to estimate sensor motion and correct IMU biases in off-line batch optimization.
  • image pixels may be received continuously over time, resulting in image distortion caused by extrinsic motion of the camera.
  • visual odometry methods may use an IMU to compensate for the rolling-shutter effect given the read-out time of the pixels.
  • GPS/INS techniques may be used to determine the position of a mobile mapping device.
  • high-accuracy GPS/INS solutions may be impractical when the application is GPS-denied, light-weight, or cost-sensitive. It is recognized that accurate GPS mapping requires line-of-sight communication between the GPS receiver and at least four GPS satellites (although five may be preferred). In some environments, it may be difficult to receive undistorted signals from four satellites, for example in urban environments that may include overpasses and other obstructions.
  • mapping device capable of acquiring optical mapping information and producing robust maps with reduced distortion.
  • a method comprises receiving data from an IMU device at a first computational module at a first frequency and computing, based at least in part on the received IMU data, a first estimated position of a mobile mapping system, receiving the first estimated position and visual-inertial data at a second computational module at a second frequency and computing, based at least in part on the first estimated position and visual-inertial data, a second estimated position of the mobile mapping system and receiving the second estimated position and laser scan data at a third computational module at a third frequency and computing, based at least in part on the second estimated position and laser scan data, a third estimated position of the mobile mapping system.
  • a mobile mapping system comprises a first computational module adapted to receive data from an IMU device at a first frequency and compute, based at least in part on the received IMU data, a first estimated position of the mobile mapping system, a second computational module adapted to receive the first estimated position and visual-inertial data at a second frequency and compute, based at least in part on the first estimated position and visual-inertial data, a second estimated position of the mobile mapping system and a third computational module adapted to receive the second estimated position and laser scan data at a third frequency and compute, based at least in part on the second estimated position and laser scan data, a third estimated position of the mobile mapping system.
  • FIG. 1 illustrates a block diagram of an embodiment of a mapping system.
  • FIG. 2 illustrates an embodiment a block diagram of the three computational modules and their respective feedback features of the mapping system of FIG. 1.
  • FIG. 3 illustrates an embodiment of a Kalmann filter model for refining positional information into a map.
  • FIG. 4 illustrates an embodiment of a factor graph optimization model for refining positional information into a map.
  • FIG. 5 illustrates an embodiment of a visual-inertial odometry subsystem.
  • FIG. 6 illustrates an embodiment of a scan matching subsystem.
  • FIG. 7A illustrates an embodiment of a large area map having coarse detail resolution.
  • FIG. 7B illustrates an embodiment of a small area map having fine detail resolution.
  • FIG. 8A illustrates an embodiment of multi-thread scan matching.
  • FIG. 8B illustrates an embodiment of single-thread scan matching.
  • FIG. 9A illustrates an embodiment of a block diagram of the three computational modules in which feedback data from the visual-inertial odometry unit is suppressed due to data degradation.
  • FIG. 9B illustrates an embodiment of the three computational modules in which feedback data from the scan matching unit is suppressed due to data degradation.
  • FIG. 10 illustrates an embodiment of the three computational modules in which feedback data from the visual-inertial odometry unit and the scan matching unit are partially suppressed due to data degradation.
  • FIG. 11 illustrates an embodiment of estimated trajectories of a mobile mapping device.
  • FIG. 12 illustrates bidirectional information flow according to an exemplary and non- limiting embodiment.
  • FIGS. 13a and 13b illustrate a dynamically reconfigurable system according to an exemplary and non-limiting embodiment.
  • FIG. 14 illustrates priority feedback for IMU bias correction according to an exemplary and non-limiting embodiment.
  • FIGS. 15a and 15b illustrate a two-layer voxel representation of a map according to an exemplary and non-limiting embodiment.
  • FIGS. 17a and 17b illustrate exemplary and non-limiting embodiments of a SLAM system.
  • FIG. 18 illustrates an exemplary and non-limiting embodiment of a SLAM enclosure.
  • FIG. 20 illustrates an exemplary and non-limiting embodiment of differing confidence level metrics.
  • FIG. 21 illustrates an exemplary and non-limiting embodiment of a SLAM system.
  • FIG. 22 illustrates an exemplary and non-limiting embodiment of timing signals for the SLAM system.
  • FIG. 23 illustrates an exemplary and non-limiting embodiment of timing signals for the SLAM system.
  • FIG. 24 illustrates an exemplary and non-limiting embodiment of SLAM system signal synchronization.
  • FIG. 25 illustrates an exemplary and non-limiting embodiment of air-ground collaborative mapping.
  • FIG. 26 illustrates an exemplary and non-limiting embodiment of a sensor pack.
  • FIG. 27 illustrates an exemplary and non-limiting embodiment of a block diagram of the laser- visual-inertial odometry and mapping software system.
  • FIG. 28 illustrates an exemplary and non-limiting embodiment of a comparison of scans involved in odometry estimation and localization.
  • FIG. 29 illustrates an exemplary and non-limiting embodiment of a comparison of scan matching accuracy in localization.
  • FIG. 30 illustrates an exemplary and non-limiting embodiment of a horizontally orientated sensor test.
  • FIG. 31 illustrates an exemplary and non-limiting embodiment of a vertically orientated sensor test.
  • FIG. 32 illustrates an exemplary and non-limiting embodiment of an accuracy comparison between horizontally orientated and downward tilted sensor tests.
  • FIG. 33 illustrates an exemplary and non-limiting embodiment of an aircraft with a sensor pack.
  • FIG. 34 illustrates an exemplary and non-limiting embodiment of sensor trajectories.
  • FIG. 35 illustrates an exemplary and non-limiting embodiment of autonomous flight results.
  • FIG. 36 illustrates an exemplary and non-limiting embodiment of an autonomous flight result over a long-run.
  • the present invention is directed to a mobile, computer-based mapping system that estimates changes in position over time (an odometer) and/or generates a three-dimensional map representation, such as a point cloud, of a three-dimensional space.
  • the mapping system may include, without limitation, a plurality of sensors including an inertial measurement unit (IMU), a camera, and/or a 3D laser scanner. It also may comprise a computer system, having at least one processor, in communication with the plurality of sensors, configured to process the outputs from the sensors in order to estimate the change in position of the system over time and/or generate the map representation of the surrounding environment.
  • IMU inertial measurement unit
  • 3D laser scanner 3D laser scanner
  • the mapping system may enable high-frequency, low-latency, on-line, real-time ego-motion estimation, along with dense, accurate 3D map registration.
  • Embodiments of the present disclosure may include a simultaneous location and mapping (SLAM) system.
  • the SLAM system may include a multi-dimensional (e.g., 3D) laser scanning and range measuring system that is GPS-independent and that provides real-time simultaneous location and mapping.
  • the SLAM system may generate and manage data for a very accurate point cloud that results from reflections of laser scanning from objects in an environment. Movements of any of the points in the point cloud are accurately tracked over time, so that the SLAM system can maintain precise understanding of its location and orientation as it travels through an environment, using the points in the point cloud as reference points for the location.
  • the resolution of the position and motion of the mobile mapping system may be sequentially refined in a series of coarse-to-fine updates.
  • discrete computational modules may be used to update the position and motion of the mobile mapping system from a coarse resolution having a rapid update rate, to a fine resolution having a slower update rate.
  • an DVIU device may provide data to a first computational module to predict a motion or position of the mapping system at a high update rate.
  • a visual-inertial odometry system may provide data to a second computational module to improve the motion or position resolution of the mapping system at a lower update rate.
  • a laser scanner may provide data to a third computational, scan matching module to further refine the motion estimates and register maps at a still lower update rate.
  • data from a computational module configured to process fine positional and/or motion resolution data may be fed back to computational modules configured to process more coarse positional and/or motion resolution data.
  • the computational modules may incorporate fault tolerance to address issues of sensor degradation by automatically bypassing computational modules associated with sensors sourcing faulty, erroneous, incomplete, or non-existent data.
  • the mapping system may operate in the presence of highly dynamic motion as well as in dark, texture-less, and structure-less environments.
  • mapping system disclosed herein can operate in real-time and generate maps while in motion. This capability offers two practical advantages. First, users are not limited to scanners that are fixed on a tripod or other nonstationary mounting. Instead, the mapping system disclosed herein may be associated with a mobile device, thereby increasing the range of the environment that may be mapped in real-time. Second, the real-time feature can give users feedback for currently mapped areas while data are collected. The online generated maps can also assist robots or other devices for autonomous navigation and obstacle avoidance. In some non-limiting embodiments, such navigation capabilities may be incorporated into the mapping system itself. In alternative non-limiting embodiments, the map data may be provided to additional robots having navigation capabilities that may require an externally sourced map.
  • the mapping system can provide point cloud maps for other algorithms that take point clouds as input for further processing. Further, the mapping system can work both indoors and outdoors. Such embodiments do not require external lighting and can operate in darkness. Embodiments that have a camera can handle rapid motion, and can colorize laser point clouds with images from the camera, although external lighting may be required.
  • the SLAM system can build and maintain a point cloud in real time as a user is moving through an environment, such as when walking, biking, driving, flying, and combinations thereof. A map is constructed in real time as the mapper progresses through an environment. The SLAM system can track thousands of features as points.
  • the SLAM system operates in real time and without dependence on external location technologies, such as GPS.
  • a plurality (in most cases, a very large number) of features of an environment, such as objects, are used as points for triangulation, and the system performs and updates many location and orientation calculations in real time to maintain an accurate, current estimate of position and orientation as the SLAM system moves through an environment.
  • relative motion of features within the environment can be used to differentiate fixed features (such as walls, doors, windows, furniture, fixtures and the like) from moving features (such as people, vehicles, and other moving items), so that the fixed features can be used for position and orientation calculations.
  • Underwater SLAM systems may use blue-green lasers to reduce attenuation.
  • mapping system design follows an observation: drift in egomotion estimation has a lower frequency than a module's own frequency.
  • the three computational modules are therefore arranged in decreasing order of frequency.
  • High-frequency modules are specialized to handle aggressive motion, while low-frequency modules cancel drift for the previous modules.
  • the sequential processing also favors computation: modules in the front take less computation and execute at high frequencies, giving sufficient time to modules in the back for thorough processing.
  • the mapping system is therefore able to achieve a high level of accuracy while running online in real-time.
  • the system may be configured to handle sensor degradation. If the camera is non-functional (for example, due to darkness, dramatic lighting changes, or texture-less environments) or if the laser is non-functional (for example due to structure-less environments) the corresponding module may be bypassed and the rest of the system may be staggered to function reliably.
  • the proposed pipeline automatically determines a degraded subspace in the problem state space, and solves the problem partially in the well-conditioned subspace. Consequently, the final solution is formed by combination of the "healthy" parts from each module. As a result, the resulting combination of modules used to produce an output is neither simply a linear or non-linear combination of module outputs.
  • the output reflect a bypass of one or more entire modules in combination with a linear or non-linear combination of the remaining functioning modules.
  • the system was tested through a large number of experiments and results show that it can produce high accuracy over several kilometers of navigation and robustness with respect to environmental degradation and aggressive motion.
  • the modularized mapping system disclosed below, is configured to process data from range, vision, and inertial sensors for motion estimation and mapping by using a multi-layer optimization structure.
  • the modularized mapping system may achieve high accuracy, robustness, and low drift by incorporating features which may include:
  • mapping system for online ego-motion estimation with data from a 3D laser, a camera, and an IMU.
  • the estimated motion further registers laser points to build a map of the traversed environment.
  • ego-motion estimation and mapping must be conducted in real-time.
  • the map may be crucial for motion planning and obstacle avoidance, while the motion estimation is important for vehicle control and maneuver.
  • FIG 1 depicts a simplified block diagram of a mapping system 100 according to one embodiment of the present invention.
  • the illustrated system includes an IMU system 102 such as an Xsens® MTi-30 IMU, a camera system 104 such as an IDS® UI-1220SE monochrome camera, and a laser scanner 106 such as a Velodyne PUCKTM VLP-16 laser scanner.
  • IMU system 102 such as an Xsens® MTi-30 IMU
  • camera system 104 such as an IDS® UI-1220SE monochrome camera
  • a laser scanner 106 such as a Velodyne PUCKTM VLP-16 laser scanner.
  • the IMU 102 may provide inertial motion data derived from one or more of an x-y-z accelerometer, a roll-pitch-yaw gyroscope, and a magnetometer, and provide inertial data at a first frequency.
  • the first frequency may be about 200 Hz.
  • the camera system 104 may have a resolution of about 752x480 pixels, a 76° horizontal field of view (FOV), and a frame capture rate at a second frequency.
  • the frame capture rate may operate at a second frequency of about 50Hz.
  • the laser scanner 106 may have a 360° horizontal FOV, a 30° vertical FOV, and receive 0.3 million points/second at a third frequency representing the laser spinning rate.
  • the third frequency may be about 5Hz.
  • the laser scanner 106 may be connected to a motor 108 incorporating an encoder 109 to measure a motor rotation angle.
  • the laser motor encoder 109 may operate with a resolution of about 0.25° ⁇
  • the IMU 102, camera 104, laser scanner 106, and laser scanner motor encoder 109 may be in data communication with a computer system 110, which may be any computing device, having one or more processors 134 and associated memory 120, 160, having sufficient processing power and memory for performing the desired odometry and/or mapping.
  • a computer system 110 may be any computing device, having one or more processors 134 and associated memory 120, 160, having sufficient processing power and memory for performing the desired odometry and/or mapping.
  • a laptop computer with 2.6GHz i7quad-core processor (2 threads on each core and 8 threads overall) and an integrated GPU memory could be used.
  • the computer system may have one or more types of primary or dynamic memory 120 such as RAM, and one or more types of secondary or storage memory 160 such as a hard disk or a flash ROM.
  • the mapping system 100 incorporates a computational model comprising individual computational modules that sequentially recover motion in a coarse-to- fine manner (see also FIG. 2).
  • a visual-inertial tightly coupled method (visual-inertial odometry module 126) estimates motion and registers laser points locally.
  • a scan matching method scan matching refinement module 132) further refines the estimated motion.
  • the scan matching refinement module 132 also registers point cloud data 165 to build a map (voxel map 134).
  • the map also may be used by the mapping system as part of an optional navigation system 136. It may be recognized that the navigation system 136 may be included as a computational module within the onboard computer system, the primary memory, or may comprise a separate system entirely.
  • each computational module may process data from one of each of the sensor systems.
  • the IMU prediction module 122 produces a coarse map from data derived from the IMU system 102
  • the visual-inertial odometry module 126 processes the more refined data from the camera system 104
  • the scan matching refinement module 132 processes the most fine-grained resolution data from the laser scanner 106 and the motor encoder 109.
  • each of the finer-grained resolution modules further process data presented from a coarser-grained module.
  • the visual-inertial odometry module 126 refines mapping data received from and calculated by the IMU prediction module 122.
  • the scan matching refinement module 132 further processes data presented by the visual inertial odometry module 126.
  • each of the sensor systems acquires data at a different rate.
  • the IMU 102 may update its data acquisition at a rate of about 200 Hz
  • the camera 104 may update its data acquisition at a rate of about 50 Hz
  • the laser scanner 106 may update its data acquisition at a rate of about 5 Hz.
  • These rates are non-limiting and may, for example, reflect the data acquisition rates of the respective sensors. It may be recognized that coarse-grained data may be acquired at a faster rate than more fine-grained data, and the coarse-grained data may also be processed at a faster rate than the fine-grained data.
  • specific frequency values for the data acquisition and processing by the various computation modules are disclosed above, neither the absolute frequencies nor their relative frequencies are limiting.
  • the mapping and/or navigational data may also be considered to comprise coarse level data and fine level data.
  • coarse positional data may be stored in a voxel map 134 that may be accessible by any of the computational modules 122, 126, 132.
  • File detailed mapping data, as point cloud data 165 that may be produced by the scan matching refinement module 132, may be stored via the processor 150 in a secondary memory 160, such as a hard drive, flash drive, or other more permanent memory.
  • both the visual-inertial odometry module 126 and the scan matching refinement module 132 can feed back their more refined mapping data to the IMU prediction module 122 via respective feedback paths 128 and 138 as a basis for updating the IMU position prediction.
  • coarse positional and mapping data may be sequentially refined in resolution, and the refined resolution data serve as feed-back references for the more coarse resolution computations.
  • FIG. 2 depicts a block diagram of the three computational modules along with their respective data paths.
  • the IMU prediction module 122 may receive IMU positional data 223 from the IMU (102, FIG. 1).
  • the visual-inertial odometry module 126 may receive the model data from the IMU prediction module 122 as well as visual data from one or more individually tracked visual features 227a, 227b from the camera (104, FIG. 1).
  • the laser scanner (106, FIG. 1) may produce data related to laser determined landmarks 233a, 233b, which may be supplied to the scan matching refinement module 132 in addition to the positional data supplied by the visual-inertial odometry module 126.
  • the positional estimation model from the visual-inertial odometry module 126 may be fed back 128 to refine the positional model calculated by the IMU prediction module 122.
  • the refined map data from the scan matching refinement module 132 may be fed back 138 to provide additional correction to the positional model calculated by the IMU prediction module 122.
  • the modularized mapping system may sequentially recover and refine motion related data in a coarse-to-fine manner.
  • the data processing of each module may be determined by the data acquisition and processing rate of each of the devices sourcing the data to the modules.
  • a visual-inertial tightly coupled method estimates motion and registers laser points locally.
  • a scan matching method further refines the estimated motion.
  • the scan matching refinement module may also register point clouds to build a map.
  • the mapping system is time optimized to process each refinement phase as data become available.
  • FIG. 3 illustrates a standard Kalman filter model based on data derived from the same sensor types as depicted in Figure 1.
  • the Kalman filter model updates positional and/or mapping data upon receipt of any data from any of the sensors regardless of the resolution capabilities of the data.
  • the positional information may be updated using the visual-inertial odometry data at any time such data become available regardless of the state of the positional information estimate based on the IMU data.
  • the Kalman filter model therefore does not take advantage of the relative resolution of each type of measurement.
  • FIG. 3 depicts a block diagram of a standard Kalman filter based method for optimizing positional data.
  • the Kalman filter updates a positional model 322a - 322n sequentially as data are presented.
  • the Kalman filter may predict 324a the subsequent positional model 322b. which may be refined based on the receive IMU mechanization data 323.
  • the positional prediction model may be updated 322b in response to the IMU mechanization data 323. in a prediction step 324a followed by update steps seeded with individual visual features or laser landmarks.
  • FIG. 4 depicts positional optimization based on a factor-graph method.
  • a pose of a mobile mapping system at a first time 410 may be updated upon receipt of data to a pose at a second time 420.
  • a factor-graph optimization model combines constraints from all sensors during each refinement calculation.
  • IMU data 323, feature data 327a, 327b, and similar from the camera, and laser landmark data 333a, 333b, and similar are all used for each update step. It may be appreciated that such a method increases the computational complexity for each positional refinement step due to the large amount of data required.
  • the sensors may provide data at independent rates that may differ by orders of magnitude, the entire refinement step is time bound to the data acquisition time for the slowest sensor.
  • FIGS. 1 and 2 sequentially recovers motion in a coarse- to-fine manner. In this manner, the degree of motion refinement is determined by the availability of each type of data.
  • a sensor system of a mobile mapping system may include a laser 106, a camera 104, and an IMU 102.
  • the camera may be modeled as a pinhole camera model for which the intrinsic parameters are known. The extrinsic parameters among all of the three sensors may be calibrated.
  • the relative pose between the camera and the laser and the relative pose between the laser and the IMU may be determined according to methods known in the art.
  • a single coordinate system may be used for the camera and the laser.
  • the camera coordinate system may be used, and all laser points may be projected into the camera coordinate system in pre-processing.
  • the IMU coordinate system may be parallel to the camera coordinate system and thus the IMU measurements may be rotationally corrected upon acquisition.
  • the coordinate systems may be defined as follows:
  • the camera coordinate system ⁇ C ⁇ may originate at the camera optical center, in which the jc-axis points to the left, the j-axis points upward, and the z-axis points forward coinciding with the camera principal axis;
  • the IMU coordinate system ⁇ / ⁇ may originate at the IMU measurement center, in which the x-, y-, and z- axes are parallel to ⁇ C ⁇ and pointing in the same directions;
  • ⁇ W ⁇ may be the coordinate system coinciding with ⁇ C ⁇ at the starting pose.
  • the landmark positions are not necessarily optimized. As a result, there remain six unknowns in the state space thus keeping computation intensity low.
  • the disclosed method involves laser range measurements to provide precise depth information to features, warranting motion estimation accuracy while further optimizing the features' depth in a bundle. One need only optimize some portion of these measurements as further optimization may result in diminishing returns in certain circumstances.
  • calibration of the described system may be based, at least in part, on the mechanical geometry of the system.
  • the LIDAR may be calibrated relative to the motor shaft using mechanical measurements from the CAD model of the system for geometric relationships between the lidar and the motor shaft.
  • Such calibration as is obtained with reference to the CAD model has been shown to provide high accuracy and drift without the need to perform additional calibration.
  • a state estimation problem can be formulated as a maximum a posterior (MAP) estimation problem.
  • MAP maximum a posterior
  • Z may be composed of both visual features and laser landmarks.
  • the joint probability of the system is defined as follows,
  • P represents the motion model
  • L represents the landmark measurement model
  • the standard way of solving Eq. 2 is to combine all sensor data, for example visual features, laser landmarks, and IMU measurements, into a large factor-graph optimization problem.
  • the proposed data processing pipeline instead, formulates multiple small optimization problems and solves the problems in a coarse-to-fine manner.
  • the optimization problem may be restated as:
  • ⁇ C ⁇ the fundamental sensor coordinate system
  • the IMU may also be characterized with respect to ⁇ C ⁇ .
  • ⁇ / ⁇ and ⁇ C ⁇ are parallel coordinate systems.
  • ⁇ a(t) and ait) may be two 3 x 1 vectors indicating the angular rates and accelerations, respectively, of ⁇ C ⁇ at time t.
  • the corresponding biases may be denoted as and and and n a (t) be the corresponding noises.
  • rotation center (origin of ⁇ C ⁇ ) is different from the origin of ⁇ / ⁇ .
  • Some examples of visual- inertial navigation methods model the motion in ⁇ / ⁇ to eliminate this centrifugal force term.
  • converting features without depth from ⁇ C ⁇ to ⁇ 7 ⁇ is not straight forward (see below).
  • the system disclosed herein models all of the motion in ⁇ C ⁇ instead. Practically, the camera and the IMU are mounted close to each other to maximally reduce effect of the term.
  • the IMU biases may be slowly changing variables. Consequently, the most recently updated biases are used for motion integration. First, Eq. 3 is integrated over time. Then, the resulting orientation is used with Eq. 4 for integration over time twice to obtain translation from the acceleration data.
  • the IMU bias correction can be made by feedback from either the camera or the laser (see 128, 138, respectively, in FIGS. 1 and 2).
  • Each feedback term contains the estimated incremental motion over a short amount of time.
  • the biases may be modeled to be constant during the incremental motion. Starting with Eq. 3, may be calculated by comparing the
  • a sliding window is employed keeping a known number of biases.
  • Non-limiting examples of the number of biases used in the sliding window may include 200 to 1000 biases with a recommended number of 400 biases based on a 200Hz IMU rate.
  • a non- limiting example of the number of biases in the sliding window with an IMU rate of 100Hz is 100 to 500 with a typical value of 200 biases.
  • the averaged biases from the sliding window are used.
  • the length of the sliding window functions as a parameter for determining an update rate of the biases.
  • the disclosed implementation is used in order to keep the IMU processing module as a separate and distinct module.
  • the sliding window method may also allow for dynamic reconfiguration of the system. In this manner, the IMU can be coupled with either the camera, the laser, or both camera and laser as required. For example, if the camera is non-functional, the IMU biases may be corrected only by the laser instead.
  • a sliding window may be employed keeping a certain number of biases.
  • the averaged biases from the sliding window may be used.
  • the length of the sliding window functions as a parameter determining an update rate of the biases.
  • the biases may be modeled as random walks and the biases updated through a process of optimization.
  • this non-standard implementation is preferred to keep IMU processing in a separate module.
  • the implementation favors dynamic reconfiguration of the system, i.e. the IMU may be coupled with either the camera or the laser. If the camera is non-functional, the IMU biases may be corrected by the laser instead.
  • inter-module communication in the sequential modularized system is utilized to fix the IMU biases. This communication enables IMU biases to be corrected.
  • IMU bias correction may be accomplished by utilizing feedback from either the camera or the laser.
  • Each of the camera and the laser contains the estimated incremental motion over a short amount of time.
  • the methods and systems described herein model the biases to be constant during the incremental motion. Still starting with Eq. 3, by comparing the estimated orientation with IMU integration, the methods and systems described herein can calculate b a (t). The updated l3 ⁇ 4(t) is used in one more round of integration to recompute the translation, which is compared with the estimated translation to calculate b a (t).
  • IMU output comprises an angular rate having relatively constant errors over time.
  • the resulting IMU bias is related to the fact that the IMU will always have some difference from ground truth. This bias can change over time. It is relatively constant and not high frequency.
  • the sliding window described above is a specified period of time during which the IMU data is evaluated.
  • a system diagram of the visual-inertial odometry subsystem couples vision with an IMU. Both vision and the IMU provide constraints to an optimization problem that estimates incremental motion. At the same time, the method associates depth information to visual features. If a feature is located in an area where laser range measurements are available, depth may be obtained from laser points. Otherwise, depth may be calculated from triangulation using the previously estimated motion sequence. As the last option, the method may also use features without any depth by formulating constraints in a different way. This is true for those features which may not necessarily have laser range coverage or cannot be triangulated due to the fact that they are not necessarily tracked long enough or located in the direction of camera motion.
  • Eigenvalues and eigenvectors may be computed and used to identify and specify degeneracy in a point cloud. If there is degeneracy in a specific direction in the state space then the solution in that direction in that state space can be discarded.
  • a block system diagram of the visual-inertial odometry subsystem is depicted in Fig. 5.
  • An optimization module 510 uses pose constraints 512 from the IMU prediction module 520 along with camera constraints 515 based on optical feature data having or lacking depth information for motion estimation 550.
  • a depthmap registration module 545 may include depthmap registration and depth association of the tracked camera features 530 with depth information obtained from the laser points 540.
  • the depthmap registration module 545 may also incorporate motion estimation 550 obtained from a previous calculation.
  • the method tightly couples vision with an IMU.
  • Each provides constraints 512, 515, respectively, to an optimization module 510 that estimates incremental motion 550.
  • the method associates depth information to visual features as part of the depthmap registration module 545.
  • depth is obtained from laser points. Otherwise, depth is calculated from triangulation using the previously estimated motion sequence.
  • the method can also use features without any depth by formulating constraints in a different way. This is true for those features which do not have laser range coverage or cannot be triangulated because they are not tracked long enough or located in the direction of camera motion.
  • the visual-inertial odometry is a key-frame based method.
  • a new key-frame is determined 535 if more than a certain number of features lose tracking or the image overlap is below a certain ratio.
  • right superscript I, I £ Z + may indicate the last key-frame, and c, c and may indicate the current frame.
  • the method combines features with and without depth.
  • a feature that is associated with depth at key-frame / may be denoted as in ⁇ C/ ⁇ .
  • a feature without depth is denoted as
  • R; and tf be the 3 x 3 rotation matrix and 3 x 1 translation vector between frames / and c, where R£ G SO(3) and and form an SE(3) transformation.
  • R(h) and t(h), h £ ⁇ 1, 2, 3 ⁇ are the ft-th rows of R; and tf.
  • di the unknown depth at key- frame /.
  • the motion estimation process 510 is required to solve an optimization problem combining three sets of constraints: 1) from features with known depth as in Eqs. 6-7; 2) from features with unknown depth as in Eq. 8; and 3) from the IMU prediction 520.
  • the normalized term 0/11 #11 represents direction of the rotation and II £>ll is the rotation angle. Each corresponds to a set of and containing 6-DOF motion of the camera.
  • a predicted transform between the last two frames c - 1 and c, denoted as ⁇ may be obtained from IMU mechanization.
  • IMU predicted translation is dependent on the orientation.
  • the orientation is dependent on the orientation.
  • accelerations that are integrated may be formulated as a function of 9f, and may
  • the 200Hz pose provided by the IMU prediction module 122 (FIGS. 1 and 2) as well as the 50Hz pose provided by the visual-inertial odometry module 126 (FIGS. 1 and 2) are both pose functions. Calculating if (0f) may begin at frame c and the accelerations may be integrated inversely with respect to time. Let Of be the rotation vector corresponding to in Eq. 5, and ⁇ [ and are the motion to be solved.
  • the pose constraints fulfill the motion model and the camera constraints fulfill the landmark measurement model in Eq. 2.
  • the optimization problem may be solved by using the Newton gradient-descent method adapted to a robust fitting framework for outlier feature removal.
  • the state space contains Q[ and if.
  • a full-scale MAP estimation is not performed, but is used only to solve a marginalized problem.
  • the landmark positions are not optimized, and thus only six unknowns in the state space are used, thereby keeping computation intensity low.
  • the method thus involves laser range measurements to provide precise depth information to features, warranting motion estimation accuracy. As a result, further optimization of the features' depth through a bundle adjustment may not be necessary.
  • the depthmap registration module 545 registers laser points on a depthmap using previously estimated motion.
  • Laser points 540 within the camera field of view are kept for a certain amount of time.
  • the depthmap is down-sampled to keep a constant density and stored in a 2D KD-tree for fast indexing.
  • KD-tree all laser points are projected onto a unit sphere around the camera center. A point is represented by its two angular coordinates.
  • features may be projected onto the sphere.
  • the three closest laser points are found on the sphere for each feature. Then, their validity may be by calculating distances among the three points in Cartesian space.
  • the depth is interpolated from the three points assuming a local planar patch in Cartesian space.
  • Those features without laser range coverage if they are tracked over a certain distance and not located in the direction of camera motion, may be triangulated using the image sequences where the features are tracked. In such a procedure, the depth may be updated at each frame based on a Bayesian probabilistic mode.
  • FIG. 6 depicts a block diagram of the scan matching subsystem.
  • the subsystem receives laser points 540 in a local point cloud and registers them 620 using provided odometry estimation 550. Then, geometric features are detected 640 from the point cloud and matched to the map. The scan matching minimizes the feature-to-map distances, similar to many methods known in the art.
  • the odometry estimation 550 also provides pose constraints 612 in the optimization 610.
  • the optimization comprises processing pose constraints with feature correspondences 615 that are found and further processed with laser constraints 617 to produce a device pose 650.
  • This pose 650 is processed through a map registration process 655 that facilitates finding the feature correspondences 615.
  • the implementation uses voxel representation of the map. Further, it can dynamically configure to run on one to multiple CPU threads in parallel.
  • the method When receiving laser scans, the method first registers points from a scan 620 into a common coordinate system. m, m e Z + may be used to indicate the scan number. It is understood that the camera coordinate system may be used for both the camera and the laser. Scan m may be associated with the camera coordinate system at the beginning of the scan, denoted as To locally register 620 the laser points 540, the odometry estimation 550 from
  • the visual-inertial odometry may be taken as key-points, and the IMU measurements may be used to interpolate in between the key-points.
  • P m be the locally registered point cloud from scan m.
  • Two sets of geometric features from P m may be extracted: one on sharp edges, namely edge points and denoted as e m , and the other on local planar surfaces, namely planar points and denoted as H m .
  • This is through computation of curvature in the local scans. Points whose neighbor points are already selected are avoided such as points on boundaries of occluded regions and points whose local surfaces are close to be parallel to laser beams. These points are likely to contain large noises or change positions over time as the sensor moves.
  • Voxels may be used to store the map truncated at a certain distance around the sensor.
  • two 3D KD-trees may be constructed, one for edge points and the other for planar points.
  • KD-trees for individual voxels accelerates point searching since given a query point, a specific KD-tree associated with a single voxel needs to be searched (see below).
  • the associated eigenvalues and eigenvectors may be examined. Specifically, one large and two small eigenvalues indicate an edge line segment, and two large and one small eigenvalues indicate a local planar patch. If the matching is valid, an equation is formulated regarding the distance from a point to the corresponding point cluster,
  • the scan matching is formulated into an optimization problem 610 minimizing the overall distances described by Eq. 12.
  • the optimization also involves pose constraints 612 from prior motion. Let be the 4 x 4 transformation matrix regarding the pose of ⁇ Cm—
  • Eq. 14 refers to the case that the prior motion is from the visual-inertial odometry, assuming the camera is functional. Otherwise, the constraints are from the IMU prediction. and may be used to denote the same terms by IMU
  • the IMU pose constraints are, where is the corresponding relative covariance matrix.
  • Eqs. 14 and 15 are linearly combined into one set of constraints. The linear combination is determined by working mode of the visual-inertial odometry.
  • the optimization problem refines and , which is solved by the Newton gradient-descent method adapted to a robust fitting framework.
  • M m _ x denotes the set of voxels 702, 704 on the first level map 700 after processing the last scan.
  • Voxels 704 surrounding the sensor 706 form a subset M m _ 1 , denoted as S m _ 1 .
  • S m _ 1 Given a 6-DOF sensor pose, and there is a corresponding S m _ 1 which moves with the sensor on the map.
  • voxels on the opposite side 725 of the boundary are moved over to extend the map boundary 730. Points in moved voxels are cleared resulting in truncation of the map.
  • each voxel j of the second level map 750 is a voxel j of the second level map 750
  • Voxels 710 are those not occupied by points from or K Upon completion of scan matching, the scan is merged into the voxels 708 with the map. After that, the map points are downsized to maintain a constant density. It may be recognized that each voxel of the first level map 700 corresponds to a volume of space that is larger than a sub-voxel of the second level map 750. Thus, each voxel of the first level map 700 comprises a plurality of sub-voxels in the second level map 750 and can be mapped onto the plurality of sub- voxels in the second level map 750.
  • first level map 700 and second level map 750 are used to store map information.
  • Voxels corresponding to M m _ 1 are used to maintain the first level map 700 and voxels corresponding to m the second level map 750 are used to retrieve the map around the
  • the map is truncated only when the sensor approaches the map boundary. Thus, if the sensor navigates inside the map, no truncation is needed.
  • Another consideration is that two KD-trees are used for each individual voxel in S m _i - one for edge points and the other for planar points. As noted above, such a data structure may accelerate point searching. In this manner, searching among multiple KD-trees is avoided as opposed to using two KD-trees for each individual voxel in The later requires more
  • Table 1 compares CPU processing time using different voxel and KD-tree configurations. The time is averaged from multiple datasets collected from different types of environments covering confined and open, structured and vegetated areas. We see that using only one level of voxels, M results in about twice of processing time for KD-tree building and querying. This is because the second level of voxels, help retrieve the
  • FIG. 8A illustrates the case where two matcher programs 812, 815 run in parallel.
  • a manager program 810 arranges it to match with the latest map available.
  • matching is slow and may not complete before arrival of the next scan.
  • the two matchers 812 and 815 are called alternatively.
  • the final motion estimation is integration of outputs from the three modules depicted in FIG. 2.
  • the 5Hz scan matching output produces the most accurate map, while the 50Hz visual-inertial odometry output and the 200Hz IMU prediction are integrated for high- frequency motion estimates.
  • the robustness of the system is determined by its ability to handle sensor degradation.
  • the IMU is always assumed to be reliable functioning as the backbone in the system.
  • the camera is sensitive to dramatic lighting changes and may also fail in a dark/texture- less environment or when significant motion blur is present (thereby causing a loss of visual features tracking).
  • the laser cannot handle structure-less environments, for example a scene that is dominant by a single plane.
  • laser data degradation can be caused by sparsity of the data due to aggressive motion.
  • Such aggressive motion comprises highly dynamic motion.
  • “highly dynamic motion” refers to substantially abrupt rotational or linear displacement of the system or continuous rotational or translational motion having a substantially large magnitude.
  • the disclosed self-motion determining system may operate in the presence of highly dynamic motion as well as in dark, texture-less, and structureless environments.
  • the system may operate while experiencing angular rates of rotation as high as 360 deg per second.
  • the system may operate at linear velocities up to and including at UOkph.
  • these motions can be coupled angular and linear motions.
  • Both the visual-inertial odometry and the scan matching modules formulate and solve optimization problems according to EQ. 2.
  • a failure happens, it corresponds to a degraded optimization problem, i.e. constraints in some directions of the problem are ill- conditioned and noise dominates in determining the solution.
  • eigenvalues denoted as 21, 22, 26, and eigenvectors, denoted as vi, v 2 , ve, associated with the problem may be computed.
  • Six eigenvalues/eigenvectors are present because the state space of the sensor contains 6-DOF (6 degrees of freedom). Without losing generality, vi, vi, ve may be sorted in decreasing order.
  • Each eigenvalue describes how well the solution is conditioned in the direction of its corresponding eigenvector.
  • well-conditioned directions may be separated from degraded directions in the state space.
  • Two matrices may be defined as:
  • the nonlinear iteration may start with an initial guess.
  • the IMU prediction provides the initial guess for the visual-inertial odometry, whose output is taken as the initial guess for the scan matching.
  • x be a solution
  • Ax be an update of x in a nonlinear iteration, in which ⁇ * is calculated by solving the linearized system equations.
  • x may be updated only in well-conditioned directions, keeping the initial guess in degraded directions instead,
  • the system solves for motion in a coarse-to-fine order, starting with the IMU prediction, the additional two modules further solving/refining the motion as much as possible. If the problem is well-conditioned, the refinement may include all 6-DOF. Otherwise, if the problem is only partially well-conditioned, the refinement may include 0 to 5-DOF. If the problem is completely degraded, V becomes a zero matrix and the previous module' s output is kept.
  • V v is a zero matrix and Eq. 18 is composed of pose constraints from the IMU prediction according to Eq. 15.
  • the MU prediction 122 bypasses the visual-inertial odometry module 126 fully or partially 924 - denoted by the dotted line -- depending on the number of well- conditioned directions in the visual-inertial odometry problem.
  • the scan matching module 132 may then locally register laser points for the scan matching.
  • the bypassing IMU prediction is subject to drift.
  • the laser feedback 138 compensates for the camera feedback 128 correcting velocity drift and biases of the IMU, only in directions where the camera feedback 128 is unavailable. Thus, the camera feedback has a higher priority, due to the higher frequency making it more suitable when the camera data are not degraded. When sufficient visual features are found, the laser feedback is not used.
  • the visual-inertial odometry module 126 output fully or partially bypasses the scan matching module to register laser points on the map 930 as noted by the dotted line. If well-conditioned directions exist in the scan matching problem, the laser feedback contains refined motion estimates in those directions. Otherwise, the laser feedback becomes empty 138.
  • FIG. 10 depicts such an example.
  • a vertical bar with six rows represents a 6- DOF pose where each row is a DOF (degree of freedom), corresponding to an eigenvector in EQ. 16.
  • the visual-inertial odometry and the scan matching each updates a 3- DOF of motion, leaving the motion unchanged in the other 3-DOF.
  • the IMU prediction 1022a- f may include initial IMU predicted values 1002.
  • the scan matching updates 1006 some 3-DOF (1032b, 1032d, 10321) resulting in a further refined prediction 1032a-1032f.
  • the camera feedback 128 contains camera updates 1028a-1028f and the laser feedback 138 contains laser updates 1038a- 1038f, respectively.
  • cells having no shading (1028a, 1028b, 1028d, 1038a, 1038c, 1038e) do not contain any updating information from the respective modules.
  • the total update 1080a- 1080f to the IMU prediction modules is a combination of the updates 1028a-1028f from the camera feedback 128 and the updates 1038a-1038f from the laser feedback 138.
  • the camera updates may have priority over the laser updates (for example 1038f).
  • the visual-inertial odometry module and the scan matching module may execute at different frequencies and each may have its own degraded directions. IMU messages may be used to interpolate between the poses from the scan matching output. In this manner, an incremental motion that is time aligned with the visual-inertial odometry output may be created.
  • ⁇ _ 1 and t be the 6-DOF motion estimated by the visual-inertial odometry between frames and be the corresponding terms estimated by the scan matching after time interpolation. and may be the matrices defined in Eq. 16 containing eigenvectors from the visual- inertial odometry module, in which v represents well-conditioned directions, and
  • a Velodyne LIDARTM HDL-32E laser scanner is attached to a UI- 1220SE monochrome camera and an Xsens ® MTi-30 IMU.
  • the laser scanner has 360° horizontal FOV, 40° vertical FOV, and receives 0.7 million points/second at 5Hz spinning rate.
  • the camera is configured at the resolution of 752 x 480 pixels, 76° horizontal FOV, and 50Hz frame rate.
  • the IMU frequency is set at 200Hz.
  • a Velodyne LIDARTM VLP-16 laser scanner is attached to the same camera and IMU. This laser scanner has 360° horizontal FOV, 30° vertical FOV, and receives 0.3 million points/second at 5Hz spinning rate.
  • Each sensor suite is attached to a vehicle for data collection, which are driven on streets and in off-road terrains, respectively.
  • the software runs on a laptop computer with a 2.6GHz i7 quad-core processor (2 threads on each core and 8 threads overall) and an integrated GPU, in a Linux ® system running Robot Operating System (ROS).
  • ROS Robot Operating System
  • Two versions of the software were implemented with visual feature tracking running on GPU and CPU, respectively.
  • the processing time is shown in Table 2.
  • the time used by the visual-inertial odometry (126 in FIG. 2) does not vary much with respect to the environment or sensor configuration.
  • For the GPU version it consumes about 25% of a CPU thread executing at 50Hz.
  • For the CPU version it takes about 75% of a thread.
  • the sensor first suite results in slightly more processing time than the second sensor suite. This is because the scanner receives more points and the program needs more time to maintain the depthmap and associate depth to the visual features.
  • the scan matching consumes more processing time which also varies with respect to the environment and sensor configuration.
  • the scan matching takes about 75% of a thread executing at 5Hz if operated in structured environments. In vegetated environments, however, more points are registered on the map and the program typically consumes about 135% of a thread.
  • the scanner receives fewer points.
  • the scan matching module 132 uses about 50-95% of a thread depending on the environment.
  • the time used by the IMU prediction (132 in FIG. 2) is negligible compared to the other two modules.
  • Tests were conducted to evaluate accuracy of the proposed system. In these tests, the first sensor suite was used. The sensors were mounted on an off-road vehicle driving around a university campus. After 2.7km of driving within 16 minutes, a campus map was built. The average speed over the test was 2.8m/s.
  • FIG. 11 depicts estimated trajectories in an accuracy test.
  • a first trajectory plot 1102 of the trajectory of a mobile sensor generated by the visual-inertial odometry system uses the IMU module 122 and the visual-inertial odometry module 126 (see FIG. 2). The configuration used in the first trajectory plot 1102 is similar to that depicted in FIG. 9B.
  • a second trajectory plot 1104 is based on directly forwarding the IMU prediction from the IMU module 122 to the scan matching module, 132 (see FIG. 2) bypassing the visual-inertial odometry. This configuration is similar to that depicted in FIG. 9A.
  • a third trajectory plot 1108 of the complete pipeline is based on the combination of the IMU module 122, the visual inertial odometry module 126, and the scan matching module 132 (see FIG. 2) has the least amount of drift.
  • the position errors of the first two configurations, trajectory plot 1102 and 1104, are about four and two times larger, respectively.
  • the first trajectory plot 1102 and the second trajectory plot 1104 can be viewed as the expected system performance when encountering individual sensor degradation. If scan matching is degraded (see FIG. 9B), the system reduces to a mode indicated by the first trajectory plot 1102. If vision is degraded, (see FIG. 9A), the system reduces to a mode indicated by the second trajectory plot 1104. If none of the sensors is degraded, (see FIG 2) the system incorporates all of the optimization functions resulting in the trajectory plot 1108. In another example, the system may take the IMU prediction as the initial guess and but run at the laser frequency (5Hz). The system produces a fourth trajectory plot 1106.
  • Another accuracy test of the system included running mobile sensor at the original lx speed and an accelerated 2x speed. When running at 2x speed, every other data frame for all three sensors is omitted, resulting in much more aggressive motion through the test. The results are listed in Table 3. At each speed, the three configurations were evaluated. At 2x speed, the accuracy of the visual-inertial odometry and the IMU + scan matching configurations reduce significantly, by 0.54% and 0.38% of the distance traveled in comparison to the accuracy at lx speed. However, the complete pipeline reduces accuracy very little, only by 0.04%. The results indicate that the camera and the laser compensate for each other keeping the overall accuracy. This is especially true when the motion is aggressive.
  • FIG. 12 there is illustrated an exemplary and non-limiting embodiment of bidirectional information flow.
  • three modules comprising an IMU prediction module, a visual-inertial odometry module and a scan-matching refinement module solve the problem step by step from coarse to fine.
  • Data processing flow is from left to right passing the three modules respectively, while feedback flow is from right to left to correct the biases of the IMU.
  • FIG. 13a With reference to Figs. 13a and 13b, there is illustrated an exemplary and non-limiting embodiment of a dynamically reconfigurable system.
  • the IMU prediction (partially) bypasses the visual-inertial odometry module to register laser points locally.
  • the visual-inertial odometry output (partially) bypasses the scan matching refinement module to register laser points on the map.
  • a vertical bar represents a 6-DOF pose and each row is a DOF.
  • the visual-inertial odometry updates in 3-DOF where the rows become designated "camera” then the scan matching updates in another 3-DOF where the rows turn designated "laser”.
  • the camera and the laser feedback is combined as the vertical bar on the left.
  • the camera feedback has a higher priority - "laser” rows from the laser feedback are only filled in if "camera” rows from the camera feedback are not present.
  • Figs. 15a and 15b there is illustrated an exemplary and non- limiting embodiment of two-layer voxel representation of a map.
  • voxels on the map M fn _ 1 all voxels in Fig. 15a
  • voxels surrounding the sensor 5 m _ 1 are a subset of M m _ 1 . If the sensor approaches the boundary of the map, voxels
  • each voxel (a dot filled voxel in Fig. 15a) is formed by a set of voxels S m 1 _ 1 that are a
  • the laser scan may
  • map points in cross-hatched voxels are extracted and stored in 3D KD-trees for scan matching.
  • Fig. 16b shows a one-thread case, where are matched with Q m , Qm- i , ....
  • the implementation is dynamically configurable using up to four threads.
  • a real time SLAM system may be used in combination with a real time navigation system.
  • the SLAM system may be used in combination with an obstacle detection system, such as a LIDAR- or RADAR-based obstacle detection system, a vision-based obstacle detection system, a thermal-based system, or the like. This may include detecting live obstacles, such as people, pets, or the like, such as by motion detection, thermal detection, electrical or magnetic field detection, or other mechanisms.
  • the point cloud that is established by scanning the features of an environment may be displayed, such as on a screen forming a part of the SLAM, to show a mapping of a space, which may include mapping of near field features, such as objects providing nearby reflections to the SLAM system, as well as far field features, such as items that can be scanned through spaces between or apertures in the near field features.
  • mapping of near field features such as objects providing nearby reflections to the SLAM system
  • far field features such as items that can be scanned through spaces between or apertures in the near field features.
  • items in an adjacent hallway may be scanned through a window or door as the mapper moves through the interior of a room, because at different points in the interior of the room different outside elements can be scanned through such spaces or apertures.
  • the resulting point cloud may then comprise comprehensive mapping data of the immediate near field environment and partial mapping of far field elements that are outside the environment.
  • the SLAM system may include mapping of a space through a "picket fence" effect by identification of far-field pieces through spaces or apertures (i.e., gaps in the fence) in the near field.
  • the far field data may be used to help the system orient the SLAM as the mapper moves from space to space, such as maintaining consistent estimation of location as the mapper moves from a comprehensively mapped space (where orientation and position are well known due to the density of the point cloud) to a sparsely mapped space (such as a new room).
  • the relative density or sparseness of the point cloud can be used by the SLAM system to guide the mapper via, for example, a user interface forming a part of the SLAM, such as directing the mapper to the parts of the far field that could not be seen through the apertures from another space.
  • the point cloud map from a SLAM system can be combined with mapping from other inputs such as cameras, sensors, and the like.
  • an airplane, drone, or other airborne mobile platform may already be equipped with other distance measuring and geo-location equipment that can be used as reference data for the SLAM system (such as linking the point cloud resulting from a scan to a GPS-referenced location) or that can take reference data from a scan, such as for displaying additional scan data as an overlay on the output from the other system.
  • conventional camera output can be shown with point cloud data as an overlay, or vice versa.
  • the SLAM system can provide a point cloud that includes data indicating the reflective intensity of the return signal from each feature.
  • This reflective intensity can be used to help determine the efficacy of the signal for the system, to determine how features relate to each other, to determine surface IR reflectivity, and the like.
  • the reflective intensity can be used as a basis for manipulating the display of the point cloud in a map.
  • the SLAM system can introduce (automatically, of under user control) some degree of color contrast to highlight the reflectivity of the signal for a given feature, material, structure, or the like.
  • the system can be married with other systems for augmenting color and reflectance information.
  • one or more of the points in the point cloud may be displayed with a color corresponding to a parameter of the acquired data, such as an intensity parameter, a density parameter, a time parameter and a geospatial location parameter.
  • Colorization of the point cloud may help users understand and analyze elements or features of the environment in which the SLAM system is operating and/or elements of features of the process of acquisition of the point cloud itself.
  • a density parameter indicating the number of points acquired in a geospatial area, may be used to determine a color that represents areas where many points of data are acquired and another color where data is sparse, perhaps suggesting the presence of artifacts, rather than "real" data.
  • Color may also indicate time, such as progressing through a series of colors as the scan is undertaken, resulting in clear indication of the path by which the SLAM scan was performed. Colorization may also be undertaken for display purposes, such as to provide differentiation among different features (such as items of furniture in a space, as compared to walls), to provide aesthetic effects, to highlight areas of interest (such as highlighting a relevant piece of equipment for attention of a viewer of a scan), and many others.
  • the SLAM system can identify "shadows" (areas where the point cloud has relatively few data points from the scan) and can (such as through a user interface) highlight areas that need additional scanning. For example, such areas may blink or be rendered in a particular color in a visual interface of a SLAM system that displays the point cloud until the shadowed area is sufficiently "painted," or covered, by laser scanning.
  • Such an interface may include any indicator (visual, text-based, voice-based, or the like) to the user that highlights areas in the field that have not yet been scanned, and any such indicator may be used to get the attention of the user either directly or through an external device (such as a mobile phone of the user).
  • the system may make reference to external data of data stored on the SLAM, such as previously constructed point clouds, maps, and the like, for comparison with current scan to identify unscanned regions.
  • the methods and systems disclosed herein include a SLAM system that provides real-time positioning output at the point of work, without requiring processing or calculation by external systems in order to determine accurate position and orientation information or to generate a map that consists of point cloud data showing features of an environment based on the reflected signals from a laser scan.
  • the methods and systems disclosed herein may also include a SLAM system that provides real time positioning information without requiring post-processing of the data collected from a laser scan.
  • a SLAM system may be integrated with various external systems, such as vehicle navigation systems (such as for unmanned aerial vehicles, drones, mobile robots, unmanned underwater vehicles, self-driving vehicles, semi-automatic vehicles, and many others).
  • vehicle navigation systems such as for unmanned aerial vehicles, drones, mobile robots, unmanned underwater vehicles, self-driving vehicles, semi-automatic vehicles, and many others.
  • the SLAM system may be used to allow a vehicle to navigate within its environments, without reliance on external systems like GPS.
  • a SLAM system may determine a level of confidence as to its current estimation of position, orientation, or the like.
  • a level of confidence may be based on the density of points that are available in a scan, the orthogonality of points available in a scan, environmental geometries or other factors, or a combination thereof.
  • the level of confidence may be ascribed to position and orientation estimates at each point along the route of a scan, so that segments of the scan can be referenced as low-confidence segments, high-confidence segments, or the like. Low-confidence segments can be highlighted for additional scanning, for use of other techniques (such as making adjustments based on external data), or the like.
  • any discrepancy between the calculated end location and the starting location may be resolved by preferentially adjusting location estimates for certain segments of the scan to restore consistency of the start- and end- locations.
  • Location and position information in low-confidence segments may be preferentially adjusted as compared to high-confidence segments.
  • the SLAM system may use confidence-based error correction for closed loop scans.
  • confidence measures with respect to areas or segments of a point cloud may be used to guide a user to undertake additional scanning, such as to provide an improved SLAM scan.
  • a confidence measure can be based on a combination of density of points, orthogonality of points and the like, which can be used to guide the user to enable a better scan.
  • scan attributes such as density of points and orthogonality of points, may be determined in real time as the scan progresses.
  • the system may sense geometries of the scanning environment that are likely to result in low confidence measures. For example, long hallways with smooth walls may not present any irregularities to differentiate one scan segment from the next. In such instances, the system may assign a lower confidence measure to scan data acquired in such environments.
  • the system can use various inputs such as LIDAR, camera, and perhaps other sensors to determine diminishing confidence and guide the user through a scan with instructions (such as "slow down,” “turn left” or the like).
  • the system may display areas of lower than desired confidence to a user, such as via a user interface, while providing assistance in allowing the user to further scan the area, volume or region of low confidence.
  • a SLAM output may be fused with other content, such as outputs from cameras, outputs from other mapping technologies, and the like.
  • a SLAM scan may be conducted along with capture of an audio track, such as via a companion application (optionally a mobile application) that captures time-coded audio notes that correspond to a scan.
  • the SLAM system provides time-coding of data collection during scanning, so that the mapping system can pinpoint when and where the scan took place, including when and where the mapper took audio and/or notes.
  • the time coding can be used to locate the notes in the area of the map where they are relevant, such as by inserting data into a map or scan that can be accessed by a user, such as by clicking on an indicator on the map that audio is available.
  • other media formats may be captured and synchronized with a scan, such as photography, HD video, or the like. These can be accessed separately based on time information, or can be inserted at appropriate places in a map itself based on the time synchronization of the scan output with time information for the other media.
  • a user may use time data to go back in time and see what has changed over time, such as based on multiple scans with different time-encoded data.
  • Scans may be further enhanced with other information, such as date- or time-stamped service record data.
  • a scan may be part of a multi-dimensional database of a scene or space, where point cloud data is associated with other data or media related to that scene, including time-based data or media.
  • calculations are maintained through a sequence of steps or segments in a manner that allows a scan to be backed up, such as to return to a given point in the scan and re-initiate at that point, rather than having to re-initiate a new scan starting at the origin. This allows use of partial scan information as a starting point for continuing a scan, such as when a problem occurs at a later point in a scan that was initially producing good output.
  • a user can "unzip” or “rewind” a scan back to a point, and then recommence scanning from that point.
  • the system can maintain accurate position and location information based on the point cloud features and can maintain time information to allow sequencing with other time-based data.
  • Time-based data can also allow editing of a scan or other media to synchronize them, such as where a scan was completed over time intervals and needs to be synchronized with other media that was captured over different time intervals.
  • Data in a point cloud may be tagged with timestamps, so that data with timestamps occurring after a point in time to which a rewind is undertaken can be erased, such that the scan can re-commence from a designated point.
  • a rewind may be undertaken to a point in time and/or to a physical location, such as rewinding to a geospatial coordinate.
  • the output from a SLAM-based map can be fused with other content, such as HD video, including by colorizing the point cloud and using it as an overlay. This may include time-synchronization between the SLAM system and other media capture system.
  • Content may be fused with video, still images of a space, a CAD model of a space, audio content captured during a scan, metadata associated with a location, or other data or media.
  • a SLAM system may be integrated with other technologies and platforms, such as tools that may be used to manipulate point clouds (e.g., CAD). This may include combining scans with features that are modeled in CAD modeling tools, rapid prototyping systems, 3D printing systems, and other systems that can use point cloud or solid model data. Scans can be provided as inputs to post-processing tools, such as colorization tools. Scans can be provided to mapping tools, such as for adding points of interest, metadata, media content, annotations, navigation information, semantic analysis to distinguish particular shapes and/or identify objects, and the like.
  • Outputs can be combined with outputs from other scanning and image-capture systems, such as ground penetrating radar, X-ray imaging, magnetic resonance imaging, computed tomography imaging, thermal imaging, photography, video, SONAR, RADAR, LID AR and the like.
  • This may include integrating outputs of scans with displays for navigation and mapping systems, such as in-vehicle navigation systems, handheld mapping systems, mobile phone navigation systems, and others.
  • Data from scans can be used to provide position and orientation data to other systems, including X, Y and Z position information, as well as pitch, roll and yaw information.
  • the data obtained from a real time SLAM system can be used for many different purposes, including for 3D motion capture systems, for acoustics engineering applications, for biomass measurements, for aircraft construction, for archeology, for architecture, engineering and construction, for augmented reality (AR), for autonomous cars, for autonomous mobile robot applications, for cleaning and treatment, for CAD/CAM applications, for construction site management (e.g., for validation of progress), for entertainment, for exploration (space, mining, underwater and the like), for forestry (including for logging and other forestry products like maple sugar management), for franchise management and compliance (e.g., for stores and restaurants), for imaging applications for validation and compliance, for indoor location, for interior design, for inventory checking, for landscape architecture, for mapping industrial spaces for maintenance, for mapping trucking routes, for military/intelligence applications, for mobile mapping, for monitoring oil pipelines and drilling, for property evaluation and other real estate applications, for retail indoor location (such as marrying real time maps to inventory maps, and the like), for security applications, for stockpile monitoring (ore, logs, goods
  • the unit comprises hardware synchronization of the IMU, camera (vision) and the LiDAR sensor.
  • the unit may be operated in darkness or structureless environments for a duration of time.
  • the processing pipeline may be comprised of modules. In darkness, the vision module may be bypassed. In structureless environments, the LiDAR module may be bypassed or partially bypassed.
  • the IMU, Lidar and camera data are all time stamped and capable of being temporally matched and synchronized. As a result, the system can act in an automated fashion to synchronize image data and point cloud data. In some instances, color data from synchronized camera images may be used to color clod data pixels for display to the user.
  • the backbone model may be generated with extra care to limit the global drift and the follow-on scans may be generated with the focus on capturing local detail. It is also possible for multiple devices to perform the detailed scanning off of the same base map for faster capture of a large region.
  • Resuming may also provide location registered temporal data. For example, multiple scans may be taken of a construction site over time to see the progress visually. In other embodiments multiple scans of a factory may help with tracking for asset management.
  • Resuming may alternately be used to purely provide localization data within the prior map. This may be useful for guiding a robotic vehicle or localizing new sensor data, such as images, thermal maps, acoustics, etc within an existing map.
  • the unit employs relatively high CPU usage in a mapping mode and relatively low CPU usage in a localization mode, suitable for long-time localization/navigation.
  • the unit supports long-time operations by executing an internal reset every once in a while. This is advantageous as some of the values generated during internal processing increase over time. Over a long period of operation (e.g., a few days), the values may reach a limit, such as a logical or physical limit of storage for the value in a computer, causing the processing, absent a reset, to potentially fail.
  • the system may automatically flush RAM memory to improve performance.
  • the system may selectively down sample older scanned data as might be necessary when performing a real time comparison of newly acquired data with older and/or archived data.
  • the unit may support a flying application and aerial-ground map merging.
  • the unit may compute a pose output at the IMU frequency, e.g., 100Hz.
  • the software may produce maps as well as sensor poses.
  • the sensor poses tell the sensor position and pointing with respect to the map being developed. High frequency and accurate pose output helps in mobile autonomy because vehicle motion control requires such data.
  • the unit further employs covariance and estimation confidence and may lock a pose when the sensor is static.
  • a SLAM SLAM
  • LIDAR is rotated to create a substantially hemispherical scan. This is performed by a mechanism with a DC motor driving a spur gear reduction to LIDAR mount via a spur gear assembly 1704.
  • the spur gear reduction assembly 1704 enables the LIDAR to be offset from the motor 1708.
  • An encoder 1706 is also in line with the rotation of the LIDAR to record the orientation of the mechanism during scanning.
  • a thin section contact bearing supports the LIDAR rotation shaft.
  • SLAM enclosure 1802 there is illustrated an exemplary and non- limiting embodiment of a SLAM enclosure 1802.
  • the SLAM enclosure 1802 is depicted in a variety of views and perspectives. Dimensions are representative of an embodiment and non- limiting as the size may be similar or different, while maintaining the general character and orientation of the major components, such as the LIDAR, odometry camera, colorization camera, user interface screen, and the like.
  • the unit may employ a neck brace, shoulder brace, carrier, or other wearable element or device (not shown), such as to help an individual hold the unit while walking around.
  • the unit or a supporting element or device may include one or more stabilizing elements to reduce shaking or vibration during the scan.
  • the unit may employ a remote battery that is carried in a shoulder bag or the like to reduce the weight of the handheld unit, whereby the scanning device has an external power source.
  • the cameras and LIDAR are arranged to maximize a field of view.
  • the camera- laser arrangement poses a tradeoff. On one side, the camera blocks the laser FOV and on the other side, the laser blocks the camera. In such an arrangement, both are blocked slightly but the blocking does not significantly sacrifice the mapping quality.
  • the camera points in the same direction as the laser because the vision processing is assisted by laser data. Laser range measurements provide depth information to the image features in the processing.
  • a confidence metric representing a confidence of spatial data.
  • Such a confidence metric measurement may include, but is not limited to, number of points, distribution of points, orthogonality of points, environmental geometry, and the like.
  • One or more confidence metrics may be computed for laser data processing (e.g., scan matching) and for image processing.
  • Figs. 19(a)- 19(c) there are illustrated exemplary and non-limiting example images showing point clouds differentiated by laser match estimation confidence. While in practice, such images may be color coded, as illustrated, both the trajectory and the points are rendered as solid or dotted in the cloud based on a last confidence value at the time of recording. In the examples, dark gray is bad and light gray is good. The values are thresholded such that everything with a value >10 is solid. Through experimentation it has been found that with a Velodyne ⁇ 1 is unreliable, ⁇ 10 is less reliable, >10 is very good.
  • Using these metrics enables automated testing to resolve model issues and offline model correction such as when utilizing a loop-closure tool as discussed elsewhere herein. Use of these metrics further enables alerting the user when matches are bad and possibly auto- pausing, throwing out low confidence data, and alerting the user when scanning.
  • Fig. 19(a) illustrates a scan of a building floor performed at a relatively slow pace.
  • Fig. 19(b) illustrates a scan of the same building floor performed at a relatively quicker pace. Note the prevalence of light fray when compared to the scan acquired from a slower scan pace arising, in part, from the speed at which the scan is conducted.
  • Fig. 19(c) illustrates a display zoomed in on a potential trouble spot of relatively low confidence.
  • Fig. 20 there is illustrated an exemplary and non-limiting embodiment of scan-to-scan match confidence metric processing and an average number of visual features that track between a full laser scan and a map being built from the prior full laser scans may be computed and presented visually. This metric may present useful, but different confidence measures.
  • a laser scan confidence metric view is presented in the left frame while an average number of visual features metric is presented in the right frame for the same data. Again, dark gray line indicates lower confidence and/or fewer average number of visual features.
  • loop closure there may be employed loop closure.
  • the unit may be operated as one walks around a room, cubicle, in and out of offices, and then back to a starting point.
  • the mesh of data from the start and end point should mesh exactly.
  • the algorithms described herein greatly minimize such drift. Typical reduction is on the order of lOx versus conventional methods (0.2% v 2%). This ratio reflects the error in distance between the start point and end point divided by the total distance traversed during the loop.
  • the software recognizes that it is back to a starting point and it can relock to the origin. Once done, one may take the variation and spread it over all of the collected data.
  • one may lock in certain point cloud data where a confidence metric indicates that the data confidence was poor and one may apply the adjustments to the areas with low confidence.
  • the system may employ both explicit and implicit loop closure.
  • a user may indicate, such as via a user interface forming a part of the SLAM, that a loop is to be closed.
  • This explicit loop closure may result in the SLAM executing software that operates to match recently scanned data to data acquired at the beginning of the loop in order to snap the beginning and end acquired data together and close the loop.
  • the system may perform implicit loop closure. In such instances, the system may operate in an automated fashion to recognize that the system is actively rescanning a location that that comprises a point or region of origin for the scan loop.
  • multi-loop confidence- based loop closure there may be performed multi-loop confidence- based loop closure.
  • structural information may be derived from the attribution of a scanned element, i.e., floors are flat, corridors are straight, etc.
  • each pixel in the camera can be mapped to a unique LIDAR pixel. For example, one may take color data from a pixel in the colorization camera corresponding to LIDAR data in the point cloud, and add the color data to the LIDAR data.
  • the unit may employ a sequential, multi-layer processing pipeline, solving for motion from coarse to fine.
  • the prior coarser result is used as an initial guess to the optimization problem.
  • the steps in the pipeline are:
  • this estimate is refined by a visual-inertial odometry optimization at the frame rate of the cameras (30-40Hz), the optimization problem uses the IMU motion estimate as an initial guess of pose change and adjusts that pose change in an attempt to minimize residual squared errors in motion between several features tracked from the current camera frame to a key frame.
  • this estimate is further refined by a laser odometry optimization at a lower rate determined by the "scan frame" rate.
  • Scan data comes in continuously, and software segments that data into frames, similar to image frames, at a regular rate, currently that rate is the time it takes for one rotation of the LIDAR rotary mechanism to make each scan frame a full hemisphere of data. That data is stitched together using visual-inertial estimates of position change as the points within the same scan frame are gathered.
  • the visual odometry estimate is taken as an initial guess and the optimization attempts to reduce residual error in tracked features in the current scan frame matched to the prior scan frame.
  • the current scan frame is matched to the entire map so far.
  • the laser odometry estimate is taken as the initial guess and the optimization minimizes residual squared errors between features in the current scan frame and features in the map so far.
  • the resulting system enables high-frequency, low-latency ego-motion estimation, along with dense, accurate 3D map registration. Further, the system is capable of handling sensor degradation by automatic reconfiguration bypassing failure modules since each step can correct errors in the prior step. Therefore, it can operate in the presence of highly dynamic motion as well as in dark, texture-less, and structure-less environments. During experiments, the system demonstrates 0.22% of relative position drift over 9.3km of navigation and robustness with respect to running, jumping and even highway speed driving (up to 33m/s).
  • Visual feature optimization w/ and w/o depth The software may attempt to determine a depth of tracked visual features, first by attempting to associate them with the laser data and secondly by attempting to triangulate depth between camera frames. The feature optimization software may then utilize all features with two different error calculations, one for features with depth and one for features without depth.
  • Laser feature determination The software may extract laser scan features as the scan line data comes in rather than in the entire scan frame. This is much easier and is done by looking at the smoothness at each point, which is defined by the relative distance between that point and the K nearest points on either side of that point then labeling the smoothest points as planar features and the sharpest as edge features. It also allows for the deletion of some points that may be bad features.
  • Map matching and voxelization A part of how the laser matching works in realtime is how the map and feature data is stored. Tracking the processor load of this stored data is critical to long term scanning and selectively voxelizing, or down-sampling into three- dimensional basic units in order to minimize the data stored while keeping what is needed for accurate matching. Adjusting the voxel sizes, or basic units, of this down-sampling on the fly based on processor load may improve the ability to maintaining real-time performance in large maps.
  • the software may be setup in such a way that it can utilize parallel processing to maintain real-time performance if data comes in faster than the processor can handle it. This is more relevant with faster point/second LIDARS like the velodyne.
  • Each optimization step in this process may provide information on the confidence in its own results.
  • the following can be evaluated to provide a measure of confidence in results: the remaining residual squared error after the optimization, the number of features tracked between frames, and the like.
  • the user may be presented a down scaled, (e.g., sub sampled) version of the multi- spectral model being prepared with data being acquired by the device.
  • each measured 3cm x 3cm x 3cm cube of model data may be represented in the scaled down version presented on the user interface as a single pixel.
  • the pixel selected for display may be the pixel that is closest to the center of the cube.
  • a representative down-scaled display being generated during operation of the SLAM is shown below.
  • the decision to display a single pixel in a volume represents a binary result indicative of either the presence of one or more points in a point cloud occupying a spatial cube of defined dimensions or the absence of any such points.
  • the selected pixel may be attributed, such as with a value indicating the number of pixels inside the defined cube represented by the selected pixel.
  • This attribute may be utilized when displaying the sub sampled point cloud such as by displaying each selected pixel utilizing color and/or intensity to reflect the value of the attribute.
  • a visual frame comprises a single 2D color image from the colorization camera.
  • a LIDAR segment comprises s full 360 degree revolution of the LIDAR scanner.
  • the visual frame and LIDAR segment are synchronized so that they can be combined and aligned with the existing model data based on the unit positional data captured from the IMU and related sensors, such as the odometry (e.g., a high speed black/white) camera.
  • the odometry e.g., a high speed black/white
  • Problems giving rise to lower confidence metrics include, but are not limited to, reflections off of glossy walls, glass, transparent glass and narrow hallways.
  • a user of the unit may pause and resume a scan such as by, for example, hitting a pause button and/or requesting a rewind to a point that is a predetermined or requested number of seconds in the past.
  • rewinding during a scan may proceed as follows.
  • the user of the system indicates a desire to rewind. This may be achieved through the manipulation of user interface forming a part of the SLAM.
  • the system deletes or otherwise removes a portion of scanned data points corresponding to a duration of time. As all scanned data points are time stamped, the system can effectively remove data points after a predetermined time, thus, "rewinding" back to a previous point in a scan.
  • the system may provide the user with a display of an image recorded at the predetermined point in time while displaying the scanned point cloud rewound to the predetermined point in time.
  • the image acts as a guide to help the user of the system reorient the SLAM into a position closely matching the orientation and pose of the SLAM at the previous predetermined point in time.
  • the user may indicate a desire to resume scanning such as by engaging a "Go" button on a user interface of the SLAM.
  • the SLAM may proceed execute a processing pipeline utilizing newly scanned data to form an initial estimation of the SLAMs position and orientation.
  • the SLAM may not add new data to the scan but, rather, may use the newly scanned data to determine and display an instantaneous confidence level of the user s position as well as a visual representation of the extent to which newly acquired data corresponds to the previous scan data.
  • scanning may continue.
  • this ability to rewind is enabled, in part, by the data being stored.
  • One may estimate how many points are brought in per second and then estimate how much to "rewind".
  • the unit may inform the user where he was x seconds a go and allow the user to move to that location and take a few scans to confirm that the user is at the appropriate place. For example, the user may be told an approximate place to go to (or the user indicate where they want to restart). If the user is close enough, the unit may figure it out where the user is and tell the user if they are close enough.
  • the unit may operate in transitions between spaces. For example, If a user walks very quickly through a narrow doorway there may not be enough data and time to determine the user's place in the new space. Specifically, in this example, the boundaries of a door frame may, prior to proceeding through it, block the LIDAR from imaging a portion of the environment beyond the door sufficient to establish a user's location. One option is to detect this lowering of confidence metric and signal to the operator to modify his behavior upon approaching a narrow passage to slow down, such as by a flashing a visual indicator or a changing the color of the screen, and the like.
  • the SLAM unit 2100 may include a timing server to generate multiple signals derived from the IMU's 2106 pulse-per- second (PPS) signal. The generated signals may be used to synchronize the data collected from the different sensors in the unit.
  • a microcontroller 2102 may be used to generate the signals and communicate with the CPU 2104.
  • the quadrature decoder 2108 may either be built into the microcontroller or on an external IC.
  • the IMU 2206 supplies a rising edge PPS signal that is used to generate the timing pulses for other parts of the system.
  • the camera may receive three signals generated from the IMU PPS signal including one rising edge signal as described above and two falling edge signals, GPIOl (lasting one frame) and GPI02 (lasting two frames as illustrated with reference to Fig. 22.
  • each camera receives a trigger signal synchronized with the IMU PPS having a high frame rate of approximately 30 Hz or 40 Hz and a high resolution of approximately 0.5 Hz - 5 Hz.
  • the encoder and the counter values may be saved together and sent to the CPU. This may happen every 40 Hz, dictated by the LIDAR synchronous output as illustrated with reference to Fig. 23.
  • An alternate time synchronization technique may include IMU based pulse-per- second synchronization that facilitates synchronizing the sensors and the computer processor.
  • IMU based pulse-per- second synchronization that facilitates synchronizing the sensors and the computer processor.
  • An exemplary and non-limiting embodiment of this type of synchronization is depicted with reference to Fig. 24.
  • the IMU 2400 may be configured to send a Pulse Per Second (PPS) signal 2406 to a LIDAR 2402. Every time a PPS is sent, the computer 2404 is notified by recognizing a flag in the IMU data stream. Then, the computer 2404 follows up and sends a time string to the LIDAR 2402. The LIDAR 2402 synchronizes to the PPS 2406 and encodes time stamps in the LIDAR data stream based on the received time strings.
  • PPS Pulse Per Second
  • the computer 2404 Upon receiving the first PPS 2406, the computer 2404 records its system time. Starting from the second PPS, the computer 2404 increases the recorded time by one second, sends the resulting time string to the LIDAR 2402, and then corrects its own system time to track the PPS 2506.
  • the IMU 2400 functions as the time server, while the initial time is obtained from the computer system time.
  • the IMU 2400 data stream is associated with time stamps based on its own clock, and initialized with the computer system time when the first PPS 2406 is sent. Therefore, the IMU2400, LIDAR 2402, and computer 2404 are all time synchronized.
  • the LIDAR 2402 may be a Velodyne LIDAR.
  • the unit includes a COM express board and a single button interface for scanning.
  • the process IMU, vision and laser data sensors may be coupled.
  • the unit may work in darkness or structureless environments for long periods of time.
  • four CPU threads may be employed for scan matching, each running at 5Hz with Velodyne data.
  • motion of the unit may be fast and the unit may localize to a prior map and can extend a map using localization.
  • the unit exhibits relatively high CPU usage in mapping mode and relatively low CPU usage in localization mode thus rendering it suitable for long-time.
  • ground-based mapping is not necessarily prone to limitations of space or time.
  • a mapping device carried by a ground vehicle is suitable for mapping in large scale and can move at a high speed.
  • a tight area can be mapped in a hand-held deployment.
  • ground-based mapping is limited by the sensor's altitude making it difficult to realize a top-down looking configuration. As illustrated in Fig. 25, the ground-based experiment produces a detailed map of the surroundings of a building, while the roof has to be mapped from the air. If a small aerial vehicle is used, aerial mapping is limited by time due to the short lifespan of batteries. Space also needs to be open enough for aerial vehicles to operate safely.
  • the collaborative mapping as described herein may utilize a laser scanner, a camera, and a low-grade IMU to process data through multi-layer optimization.
  • the resulting motion estimates may be at a high rate ( ⁇ 200Hz) with a low drift (typically ⁇ 0.1% of the distance traveled).
  • the high-accuracy processing pipeline described herein may be utilized to merge maps generated from the ground with maps generated from the air in real-time or near realtime. This is achieved, in part, by localization of one output from a ground derived map with respect to an output from an air derived map.
  • the method disclosed herein fulfills collaborative mapping, it further reduces the complexity of aerial deployments.
  • a ground-based map flight paths are defined and an aerial vehicle conducts mapping in autonomous missions.
  • the aerial vehicle is able to accomplish challenging flight tasks autonomously.
  • the processing software is not necessarily limited to a particular sensor configuration.
  • the sensor pack 2601 is comprised of a laser scanner 2603 generating 0.3 million points/second, a camera 2605 at 640 x 360 pixels resolution and 50Hz frame rate, and a low-grade IMU 2607 at 200Hz.
  • An onboard computer processes data from the sensors in real-time for ego-motion estimation and mapping.
  • Fig. 26(b) and Fig. 26(c) illustrate the sensor field of view. An overlap is shared by the laser and camera, with which, the processing software associates depth information from the laser to image features as described more fully below.
  • the software processes data from a range sensor such as a laser scanner, a camera, and an inertial sensor.
  • a range sensor such as a laser scanner, a camera, and an inertial sensor.
  • the methods and systems described herein parse the problem as multiple small problems, solve them sequentially in a coarse-to-fine manner.
  • Fig. 27 illustrates a block diagram of the software system. In such a system, modules in the front conduct light processing, ensuring high- frequency motion estimation robust to aggressive motion. Modules in the back take sufficient processing, run at low frequencies to warrant accuracy of the resulting motion estimates and maps.
  • the software starts with IMU data processing 2701.
  • This module runs at the IMU frequency to predict the motion based on IMU mechanization.
  • the result is further processed by a visual-inertial coupled module 2703.
  • the module 2703 tracks distinctive image features through the image sequence and solves for the motion in an optimization problem.
  • laser range measurements are registered on a depthmap, with which, depth information is associated to the tracked image features. Since the sensor pack contains a single camera, depth from the laser helps solve scale ambiguity during motion estimation.
  • the estimated motion is used to register laser scans locally.
  • these scans are matched to further refine the motion estimates.
  • the matched scans are registered on a map while scans are matched to the map.
  • scan matching utilizes multiple CPU threads in parallel.
  • the map is stored in voxels to accelerate point query during scan matching. Because the motion is estimated at different frequencies, a fourth module 2707 in the system takes these motion estimates for integration. The output holds both high accuracy and low latency beneficial for vehicle control.
  • the modularized system also ensures robustness with respect to sensor degradation, by selecting "healthy" modes of the sensors when forming the final solution. For example, when a camera is in a low-light or texture-less environment such as pointing to a clean and white wall, or a laser is in a symmetric or extruded environment such as a long and straight corridor, processing typically fails to generate valid motion estimates.
  • the system may automatically determine a degraded subspace in the problem state space. When degradation happens, the system only solves the problem partially in the well-conditioned subspace of each module. The result is that the "healthy" parts are combined to produce the final, valid motion estimates.
  • the method described above can be extended to utilize the map for localization. This is accomplished using a scan matching method.
  • the method extracts two types of geometric features, specifically, points on edges and planar surfaces, based on the curvature in local scans. Feature points are matched to the map. An edge point is matched to an edge line segment, and a planar point is matched to a local planar patch. On the map, the edge line segments and local planar patches are determined by examining the eigenvalues and eigenvectors associated with local point clusters.
  • the map is stored in voxels to accelerate processing.
  • the localization solves an optimization problem minimizing the overall distances between the feature points and their correspondences. Due to the fact that the high accuracy odometry estimation is used to provide initial guess to the localization, the optimization usually converges in 2-3 iterations.
  • the localization does not necessarily process individual scans but, rather, stacks a number of scans for batch processing. Thanks to the high-accuracy odometry estimation, scans are registered precisely in a local coordinate frame where drift is negligible over a short period of time (e.g., a few seconds).
  • Fig. 28 (8.4) where Fig. 28(a) is a single scan that is matched in the previous section (scan matching executes at 5Hz), and Fig. 28(b) shows stacked scans over two seconds, which are matched during localization (scan matching runs at 0.5Hz).
  • Fig. 28(a) is a single scan that is matched in the previous section (scan matching executes at 5Hz)
  • Fig. 28(b) shows stacked scans over two seconds, which are matched during localization (scan matching runs at 0.5Hz).
  • the stacked scans contain significantly more structural details, contributing to the localization accuracy and robustness with respect to environmental changes.
  • the localization is compared to a particle filter based implementation.
  • the odometry estimation provides the motion model to the particle filter. It uses a number of 50 particles. At each update step, the particles are resampled based on low-variance resampling. Comparison results are shown in Fig. 29 and Table 8.1.
  • errors are defined as the absolute distances from localized scans to the map.
  • the methods and systems described herein choose a number of planar surfaces and use the distances between points in localized scans to the corresponding planar patches on the map.
  • Fig. 29 illustrates an exemplary and non-limiting embodiment of an error distribution. When running the particle filter at the same frequency as the described method (0.5Hz), the resulting error is five times as large.
  • Fig. 30(a) shows the map built and sensor trajectory.
  • Fig. 30(b) is a single scan. In this scenario, the scan contains sufficient structural information. When bypassing the camera processing module, the system produces the same trajectory as the full pipeline.
  • the methods and systems described herein run another test with the sensor pack tilted vertically down toward the ground. The results are shown in Fig. 31. In this scenario, structural information in a scan is much sparser as shown in Fig. 31(b)). The processing fails without usage of the camera and succeeds with the full pipeline. The results indicate the camera is critical for high-altitude flights where tilting of the sensor pack is required.
  • FIG. 32 there is illustrated an exemplary and non-limiting embodiment wherein the sensor pack is held by an operator walking through a circle at l-2m/s speed with an overall traveling distance of 410m.
  • Fig. 32(a) shows the resulting map and sensor trajectory with a horizontally orientated sensor configuration. The sensor is started and stopped at the same position. The test produces 0.18m of drift through the path, resulting in 0.04% of relative position error in comparison to the distance traveled. Then, the operator repeats the path with two sensor packs held at 45° and 90° angles, respectively. The resulting sensor trajectories are shown in Fig. 32(b).
  • FIG. 33 An exemplary and non-limiting embodiment of a drone platform 3301 is illustrated at Fig. 33.
  • the aircraft weighs approximately 6.8kg (including batteries) and may carry a maximum of 4.2kg payload.
  • the sensor/computer pack is mounted to the bottom of the aircraft, weighting 1.7kg.
  • the bottom right of the figure shows the remote controller.
  • the remote controller is operated by a safety pilot to override the autonomy if necessary.
  • the aircraft is built with a GPS receiver (on top of the aircraft). GPS data is not necessarily used in mapping or autonomous.
  • Fig. 25 In the first collaborative mapping experiment, an operator holds the sensor pack and walks around a building. Results are shown in Fig. 25.
  • the ground-based mapping covers surroundings of the building in detail, conducted at l-2m/s over 914m of travel. As expected, the roof of the building is empty on the map.
  • the drone is teleoperated to fly over the building.
  • the flight In Fig. 25(b), the flight is conducted at 2-3m/s with a traveling distance of 269m.
  • the processing uses localization w.r.t. the map in Fig. 25(a). That way, the aerial map is merged with the ground-based map (white points).
  • the take-off position of the drone is determined on the map.
  • the sensor starting pose for the aerial mapping is known, and from which, the localization starts.
  • Fig. 34 presents the aerial and ground-based sensor trajectories, in top-down and side views.
  • a ground-based map is built first by handheld mapping at l-2m/s for 672m of travel around the flight area.
  • the map and sensor trajectory are shown in Fig. 35(a).
  • way-points are defined and the drone follows the way-points to conduct aerial mapping.
  • the curve is the flight path
  • the large points on the curve are the way-points
  • the points form the aerial map.
  • the drone takes off inside a shed on the left side of the figure, flies across the site and passes through another shed on the right side, then returns to the first shed to land.
  • Fig. 35(c) and Fig. 35(d) are two images taken by an onboard camera when the drone flies toward the shed on the right and is about to enter the shed.
  • Fig. 35(e) shows the estimated speed during the mission.
  • the ground-based mapping involves an off -road vehicle driven at lOm/s from the left end to the right end, over 1463m of travel. With the ground-based map and way-points, the autonomous flight crosses the site. Upon take-off, the drone ascends to 20m high above the ground at 15m/s. Then, it descends to 2m above the ground to fly through a line of trees at lOm/s. The flight path is 1118m long as indicated by the curve 3601 in Fig. 36(b). Two images are taken as the drone flies high above the trees (see Fig. 36(c)) and low underneath the trees (see Fig. 36(d)).
  • global positioning data such as from a GPS may be incorporated into the processing pipeline.
  • Global positioning data can be helpful to cancel ego-motion estimation drift over a long distance of travel and register maps in a global coordinate frame.
  • GPS data may be recorded simultaneously with mapping activity.
  • the system moves there is some level of drift that causes an error to grow over distance.
  • One may typically experience only 0.2% drift rate but when traveling 1000 meters that is still 2 meters for every 1000 meters of travel. At 10km this grows to 20 meters, etc. Without closing the loop (in the traditional sense of coming back to the beginning of the route) this error cannot be corrected.
  • the system can know where it is and correct the current position estimate. While this is typically done in a post-processing effort, the present system is able to accomplish such a correction in real-time or near real-time.
  • GPS provides a method by which one may close the loop.
  • GPS has some amount of error as well, but it is usually consistent in a given area and many GPS systems today can provide better than 30cm accuracy in position X and Y on the surface. Other more expensive and sophisticated systems can provide cm level positioning.
  • GPS provides important capabilities: 1. The location of the point cloud on the planet. 2. the ability to use the course-corrected information to align and "fix" the map so that the map becomes even more accurate since one knows one's position and any data taken at that position may now be referenced to the series of GPS points that are also collected. 3. The ability for the system to act as an IMU when GPS is lost.
  • dynamic vision sensors may be utilized to further improve estimation robustness with respect to aggressive motion.
  • a dynamic vision sensor reports data only on pixels with illumination changes, delivering both a high rate and a low latency.
  • This high rate (typically defined as more than approximately 10Hz) may provide rapid information quickly to the ego-motion and estimation system thus improving values for localization and, subsequently, mapping. If the system is able to capture more data with fewer delays, the system will be more accurate, and more robust.
  • the features that are identified and tracked by the dynamic vision sensor enable better estimates since more features, and faster updates enable more accurate tracking and motion estimation.
  • Direct methods may be used to realize image matching with a dynamic vision sensor for ego-motion estimation. Specifically, direct methods match sequential images for feature tracking from image to image. In contrast, the feature tracking method disclosed herein is superior to the direct method.
  • parallel processing may be implemented to execute on a general purpose GPU or FPGA and therefore enable data processing in larger amount and higher frequencies.
  • Parallel architectures may take the form of multiple cores, thread, processors or even specialized forms such as Graphics Processing Units (GPUs).
  • GPUs Graphics Processing Units
  • loop closures may be introduced to remove ego-motion estimation drift by global smoothing.
  • Another way may be to use some other measure of error during the traverse and applying corrections for only those places where the measure of error is high.
  • the covariance matrix provides a convenient metric for map quality and may be used to proactively distribute this error over the full traverse.
  • locations with low-quality matching that show up in the covariance matrix could be used to unevenly spread the error across the traverse in accordance with the proportion of the error over that traverse. This will act to correct the error in the precise areas where low-quality values were associated with particular locations.
  • a method comprising: acquiring a LIDAR point cloud comprising a plurality of points each of which are attributed with at least a geospatial coordinate and a segment, assigning a confidence level to each segment indicative of a computed accuracy of the plurality of points attributed with the same segment and adjusting the geospatial coordinate of each of at least a portion of the plurality of points attributed with the same segment based, at least in part, on a confidence level.
  • Clause 4 The method of clause 1, wherein a segment for adjusting the geospatial coordinate has a lower confidence level than at least one other segment.
  • a method comprising: commencing to acquire a LIDAR point cloud with a SLAM the point cloud having a starting location and comprising a plurality of points each of which are attributed with at least a geospatial coordinate and a segment, traversing a loop while acquiring the LIDAR point cloud and determining a scan end point when the SLAM is in proximity to the starting location.
  • Clause 8 The method of clause 6, wherein points in a segment other than a segment comprising the starting location are attributed with a geospatial coordinate that is proximal to the starting location.
  • a method comprising: acquiring a LIDAR point cloud comprising a plurality of points each of which are attributed with at least a geospatial coordinate and a timestamp, acquiring color image data comprising a plurality of images each of which are attributed with at least one of the geospatial coordinates and the timestamp and colorizing at least a portion of the plurality of points with color information derived from an image having a timestamp that is close in time to the timestamp of each point being colorized and having a geospatial coordinate that is close in proximity to the geospatial coordinate of the colorized plurality of points.
  • a method comprising: deriving a motion estimate for a SLAM system using an IMU forming a part of the SLAM system, refining the motion estimate via a visual- inertial odometry optimization process to produce a refined estimate and refining the refined estimate via a laser odometry optimization process by minimizing at least one residual squared error between at least one feature in a current scan and at least one previously scanned feature.
  • a method comprising: acquiring a plurality of depth tracked visual features in a plurality of camera frames using a camera forming a part of a SLAM system, associating the plurality of visual features with a LIDAR derived point cloud acquired from a LIDAR forming a part of the SLAM and triangulating a depth of at least one visual feature between at least two camera frames.
  • Clause 18 The method of clause 17, where in the associating and triangulating steps are performed on a processor employing parallel computing.
  • a SLAM device comprising: a microcontroller, an inertial measurement unit (IMU) adapted to produce a plurality of timing signals and a timing server adapted to generate a plurality of synchronization signals derived from the plurality of timing signals, wherein the synchronization signals operate to synchronize at least two sensors forming a part of the SLAM device.
  • IMU inertial measurement unit
  • a timing server adapted to generate a plurality of synchronization signals derived from the plurality of timing signals, wherein the synchronization signals operate to synchronize at least two sensors forming a part of the SLAM device.
  • a method comprising: acquiring a LIDAR point cloud comprising a plurality of points each of which are attributed with at least a geospatial coordinate and a timestamp, acquiring color image data comprising a plurality of images each of which are attributed with at least the geospatial coordinate and the timestamp, colorizing at least a portion of the plurality of points with color information derived from an image having at least one of a timestamp that is close in time to the timestamp of each point being colorized and a geospatial coordinate that is close in distance to the geospatial coordinate of each point being colorized and displaying the colorized portion of the plurality of points.
  • Clause 22 The method of clause 21, further comprising displaying output from a camera as an overlay on the displayed plurality of points.
  • a method comprising acquiring a LIDAR point cloud comprising a plurality of points each of which are attributed with at least a geospatial coordinate and a timestamp and colorizing at least a portion of the plurality of points with color information, wherein each of the plurality of points is colorized with a color corresponding to a parameter of the acquired LIDAR point cloud data selected from the group consisting of an intensity parameter, a density parameter, a time parameter and a geospatial location parameter.
  • a method comprising: acquiring a LIDAR point cloud with a SLAM comprising a plurality of near field points derived from a corresponding near environment and a plurality of far field points derived from a corresponding far field environment wherein the far field points are scanned through one or more spaces between one or more elements located in the near environment and utilizing the plurality of far field points to orient the SLAM as it moves from the near environment to the far environment.
  • a method comprising receiving feedback comprising a plurality of feedback terms at a SLAM system from at least one of a camera and a laser and modeling a plurality of biases each associated with one of the plurality of feedback terms wherein the plurality of biases form a sliding window of biases.
  • each of the plurality of feedback terms comprises an estimated incremental motion of the SLAM system.
  • each of the plurality of biases term comprises is modeled to be constant during the incremental motion.
  • Clause 30 The method of clause 25, wherein a length of the sliding window functions as a parameter for determining an update rate of the plurality of biases.
  • Clause 33 The method of clause 32, wherein performing IMU bias correction comprises utilizing data from at least one of a laser and a camera.
  • Clause 34 The method of clause 33 wherein the laser and the camera form a part of the SLAM system.
  • a method comprising receiving vision data from a camera and inertial data from an IMU the camera and the IMU forming a part of a SLAM system, estimating incremental motion of the SLAM system using the vision data and inertial data as constraints and associating depth information with one or more visual features derived from the vision data.
  • Clause 40 The method of clause 37, wherein the depth information is utilized to build a registered point cloud.
  • Clause 41 The method of clause 40, further comprising computing one or more eigenvectors for the point cloud and using the one or more eigenvectors to specify a degeneracy of the point cloud.
  • Clause 42 The method of clause 41, wherein degeneracy in a direction of a state space as indicated by at least one of the eigenvectors is utilized to discard a solution in the direction.
  • Clause 43 A method comprising determining a relative pose between a camera having a camera coordinate system and a laser having a laser coordinate system both forming a part of a SLAM system by utilizing a single coordinate system for the camera and the laser and determining a relative pose between the laser and an IMU forming a part of a SLAM system and having an IMU coordinate system.
  • Clause 44 The method of clause 43, wherein utilizing the single coordinate system further comprises projecting a plurality of laser points into the camera coordinate system during pre-processing.
  • Clause 46 The method of clause 45, further comprising rotationally correcting IMU data from the IMU upon an acquisition of the IMU data.
  • a method comprising establishing a motion model of a mobile mapping system utilizing one or more pose constraints, establishing a landmark measurement model of a mobile mapping system the landmark measurement model comprising one or more landmark positions utilizing one or more camera constraints and solving for each of the motion model and landmark measurements.
  • Clause 51 The method of clause 50, wherein the solving of each model comprises utilizing a Newton gradient-descent method.
  • Clause 52 The method of clause 51 , wherein the Newton gradient-descent method adapted to a robust fitting framework for outlier feature removal.
  • Clause 53 The method of clause 50, wherein the solving in performed without optimizing the plurality of landmark positions.
  • a method comprising receiving an odometry estimation from a visual- inertial odometry module of a mobile mapping system comprising a plurality of key points, receiving a plurality of IMU measurements from an IMU forming a part of the mobile mapping system and registering a plurality of laser points gathered from a laser forming a part of the mobile mapping system based, at least in part, on the IMU measurements.
  • Clause 55 The method of clause 54, wherein the registering comprises utilizing IMU measurements to interpolate between a plurality of key points.
  • Clause 58 The method of clause 56, wherein bad points in the point cloud are not selected based upon a relationship with one or more points and surfaces in the point cloud.
  • a method comprising storing map information comprising a point cloud in a plurality of first level voxels each occupying an identically sized first volume, storing the map information in a plurality of second level voxels each occupying an identically sized second volume wherein the first volume that is larger than the second volume, retrieving map information from the second level voxels in proximity to a laser scanner of a mobile mapping system, performing scan matching on the retrieved second level voxels and maintaining the map information comprised of first level voxels using the first level voxels.
  • Clause 60 The method of clause 59, where in each first level voxel is mapped to a plurality of second level voxels.
  • Clause 61 The method of clause 59, wherein the second level voxels are stored in a 3D KD-tree.
  • Clause 62 The method of claim 59, further comprising downsizing the point cloud to maintain a near constant point density.
  • a method comprising providing a ground generated point cloud map, generating an air generated point cloud map and merging in real or near-real time the ground generated point cloud map and the air generated point cloud map.
  • Clause 64 The method of clause 63, wherein the air generated point cloud map is generated utilizing a drone comprising a mobile mapping system.
  • Clause 65 The method of clause 63, wherein the drone comprises a laser scanner, a camera and a low-grade IMU.
  • Clause 66 The method of clause 65, wherein data from the laser scanner, the camera and the IMU is processed via a multi-layer optimization process.
  • Clause 67 The method of clause 63, wherein the merging further comprises localizing at least one output from the ground derived map with respect to an output from the air generated point cloud map.
  • the methods and systems described herein may be deployed in part or in whole through a machine that executes computer software, program codes, and/or instructions on a processor.
  • the present disclosure may be implemented as a method on the machine, as a system or apparatus as part of or in relation to the machine, or as a computer program product embodied in a computer readable medium executing on one or more of the machines.
  • the processor may be part of a server, cloud server, client, network infrastructure, mobile computing platform, stationary computing platform, or other computing platform.
  • a processor may be any kind of computational or processing device capable of executing program instructions, codes, binary instructions and the like.
  • the processor may be or may include a signal processor, digital processor, embedded processor, microprocessor or any variant such as a co-processor (math co-processor, graphic co-processor, communication co-processor and the like) and the like that may directly or indirectly facilitate execution of program code or program instructions stored thereon.
  • the processor may enable execution of multiple programs, threads, and codes. The threads may be executed simultaneously to enhance the performance of the processor and to facilitate simultaneous operations of the application.
  • methods, program codes, program instructions and the like described herein may be implemented in one or more thread.
  • the thread may spawn other threads that may have assigned priorities associated with them; the processor may execute these threads based on priority or any other order based on instructions provided in the program code.
  • the processor may include non- transitory memory that stores methods, codes, instructions and programs as described herein and elsewhere.
  • the processor may access a non-transitory storage medium through an interface that may store methods, codes, and instructions as described herein and elsewhere.
  • the storage medium associated with the processor for storing methods, programs, codes, program instructions or other type of instructions capable of being executed by the computing or processing device may include but may not be limited to one or more of a CD-ROM, DVD, memory, hard disk, flash drive, RAM, ROM, cache and the like.
  • a processor may include one or more cores that may enhance speed and performance of a multiprocessor.
  • the process may be a dual core processor, quad core processors, other chip-level multiprocessor and the like that combine two or more independent cores (called a die).
  • the methods and systems described herein may be deployed in part or in whole through a machine that executes computer software on a server, client, firewall, gateway, hub, router, or other such computer and/or networking hardware.
  • the software program may be associated with a server that may include a file server, print server, domain server, internet server, intranet server, cloud server, and other variants such as secondary server, host server, distributed server and the like.
  • the server may include one or more of memories, processors, computer readable media, storage media, ports (physical and virtual), communication devices, and interfaces capable of accessing other servers, clients, machines, and devices through a wired or a wireless medium, and the like.
  • the methods, programs, or codes as described herein and elsewhere may be executed by the server.
  • other devices required for execution of methods as described in this application may be considered as a part of the infrastructure associated with the server.
  • the server may provide an interface to other devices including, without limitation, clients, other servers, printers, database servers, print servers, file servers, communication servers, distributed servers, social networks, and the like. Additionally, this coupling and/or connection may facilitate remote execution of program across the network. The networking of some or all of these devices may facilitate parallel processing of a program or method at one or more location without deviating from the scope of the disclosure.
  • any of the devices attached to the server through an interface may include at least one storage medium capable of storing methods, programs, code and/or instructions.
  • a central repository may provide program instructions to be executed on different devices.
  • the remote repository may act as a storage medium for program code, instructions, and programs.
  • the software program may be associated with a client that may include a file client, print client, domain client, internet client, intranet client and other variants such as secondary client, host client, distributed client and the like.
  • the client may include one or more of memories, processors, computer readable media, storage media, ports (physical and virtual), communication devices, and interfaces capable of accessing other clients, servers, machines, and devices through a wired or a wireless medium, and the like.
  • the methods, programs, or codes as described herein and elsewhere may be executed by the client.
  • other devices required for execution of methods as described in this application may be considered as a part of the infrastructure associated with the client.
  • the client may provide an interface to other devices including, without limitation, servers, other clients, printers, database servers, print servers, file servers, communication servers, distributed servers and the like. Additionally, this coupling and/or connection may facilitate remote execution of program across the network. The networking of some or all of these devices may facilitate parallel processing of a program or method at one or more location without deviating from the scope of the disclosure.
  • any of the devices attached to the client through an interface may include at least one storage medium capable of storing methods, programs, applications, code and/or instructions.
  • a central repository may provide program instructions to be executed on different devices.
  • the remote repository may act as a storage medium for program code, instructions, and programs.
  • the methods and systems described herein may be deployed in part or in whole through network infrastructures.
  • the network infrastructure may include elements such as computing devices, servers, routers, hubs, firewalls, clients, personal computers, communication devices, routing devices and other active and passive devices, modules and/or components as known in the art.
  • the computing and/or non-computing device(s) associated with the network infrastructure may include, apart from other components, a storage medium such as flash memory, buffer, stack, RAM, ROM and the like.
  • the processes, methods, program codes, instructions described herein and elsewhere may be executed by one or more of the network infrastructural elements.
  • SaaS software as a service
  • PaaS platform as a service
  • IaaS infrastructure as a service
  • the methods, program codes, and instructions described herein and elsewhere may be implemented on a cellular network has sender-controlled contact media content item multiple cells.
  • the cellular network may either be frequency division multiple access (FDMA) network or code division multiple access (CDMA) network.
  • FDMA frequency division multiple access
  • CDMA code division multiple access
  • the cellular network may include mobile devices, cell sites, base stations, repeaters, antennas, towers, and the like.
  • the cell network may be a GSM, GPRS, 3G, EVDO, mesh, or other networks types.
  • the methods, program codes, and instructions described herein and elsewhere may be implemented on or through mobile devices.
  • the mobile devices may include navigation devices, cell phones, mobile phones, mobile personal digital assistants, laptops, palmtops, netbooks, pagers, electronic books readers, music players and the like. These devices may include, apart from other components, a storage medium such as a flash memory, buffer, RAM, ROM and one or more computing devices.
  • the computing devices associated with mobile devices may be enabled to execute program codes, methods, and instructions stored thereon. Alternatively, the mobile devices may be configured to execute instructions in collaboration with other devices.
  • the mobile devices may communicate with base stations interfaced with servers and configured to execute program codes.
  • the mobile devices may communicate on a peer-to-peer network, mesh network, or other communications network.
  • the program code may be stored on the storage medium associated with the server and executed by a computing device embedded within the server.
  • the base station may include a computing device and a storage medium.
  • the storage device may store program codes and instructions executed by the computing devices associated with the base station.
  • the computer software, program codes, and/or instructions may be stored and/or accessed on machine readable media that may include: computer components, devices, and recording media that retain digital data used for computing for some interval of time; semiconductor storage known as random access memory (RAM); mass storage typically for more permanent storage, such as optical discs, forms of magnetic storage like hard disks, tapes, drums, cards and other types ; processor registers, cache memory, volatile memory, non- volatile memory; optical storage such as CD, DVD; removable media such as flash memory (e.g.
  • USB sticks or keys floppy disks, magnetic tape, paper tape, punch cards, standalone RAM disks, Zip drives, removable mass storage, off-line, and the like; other computer memory such as dynamic memory, static memory, read/write storage, mutable storage, read only, random access, sequential access, location addressable, file addressable, content addressable, network attached storage, storage area network, bar codes, magnetic ink, and the like.
  • the methods and systems described herein may transform physical and/or or intangible items from one state to another.
  • the methods and systems described herein may also transform data representing physical and/or intangible items from one state to another.
  • machines may include, but may not be limited to, personal digital assistants, laptops, personal computers, mobile phones, other handheld computing devices, medical equipment, wired or wireless communication devices, transducers, chips, calculators, satellites, tablet PCs, electronic books, gadgets, electronic devices, devices has sender-controlled contact media content item artificial intelligence, computing devices, networking equipment, servers, routers and the like.
  • the elements depicted in the flow chart and block diagrams or any other logical component may be implemented on a machine capable of executing program instructions.
  • the methods and/or processes described above, and steps associated therewith, may be realized in hardware, software or any combination of hardware and software suitable for a particular application.
  • the hardware may include a general- purpose computer and/or dedicated computing device or specific computing device or particular aspect or component of a specific computing device.
  • the processes may be realized in one or more microprocessors, microcontrollers, embedded microcontrollers, programmable digital signal processors or other programmable device, along with internal and/or external memory.
  • the processes may also, or instead, be embodied in an application specific integrated circuit, a programmable gate array, programmable array logic, or any other device or combination of devices that may be configured to process electronic signals.
  • one or more of the processes may be realized as a computer executable code capable of being executed on a machine -readable medium.
  • the computer executable code may be created using a structured programming language such as C, an object oriented programming language such as C++, or any other high- level or low-level programming language (including assembly languages, hardware description languages, and database programming languages and technologies) that may be stored, compiled or interpreted to run on one of the above devices, as well as heterogeneous combinations of processors, processor architectures, or combinations of different hardware and software, or any other machine capable of executing program instructions.
  • methods described above and combinations thereof may be embodied in computer executable code that, when executing on one or more computing devices, performs the steps thereof.
  • the methods may be embodied in systems that perform the steps thereof, and may be distributed across devices in a number of ways, or all of the functionality may be integrated into a dedicated, standalone device or other hardware.
  • the means for performing the steps associated with the processes described above may include any of the hardware and/or software described above. All such permutations and combinations are intended to fall within the scope of the present disclosure.

Abstract

A method includes receiving data from an IMU device at a first computational module at a first frequency and computing, based at least in part on the received IMU data, a first estimated position of a mobile mapping system, receiving the first estimated position and visual-inertial data at a second computational module at a second frequency and computing, based at least in part on the first estimated position and visual-inertial data, a second estimated position of the mobile mapping system and receiving the second estimated position and laser scan data at a third computational module at a third frequency and computing, based at least in part on the second estimated position and laser scan data, a third estimated position of the mobile mapping system.

Description

LASER SCANNER WITH REAL-TIME, ONLINE EGO-MOTION ESTIMATION
STATEMENT OF PRIORITY
[0001] This application claims priority to, and is a continuation-in-part of, PCT Application No. PCT/US2017/055938 (Atty. Dckt. No. KRTA-0008-WO) entitled "LASER SCANNER WITH REAL-TIME, ONLINE EGO-MOTION ESTIMATION," filed on October 10, 2017.
[0002] PCT Application No. PCT/US2017/055938 claims priority to, and is a continuation-in-part of, PCT Application No. PCT/US2017/021120 (Atty. Dckt. No. KRTA- 0005 -WO) entitled "LASER SCANNER WITH REAL-TIME, ONLINE EGO-MOTION ESTIMATION," filed on March 7, 2017. This application likewise claims priority to PCT Application No. PCT/US2017/021120. PCT Application No. PCT/US2017/055938 further claims priority to U.S. Provisional No. 62/406,910 (Atty. Dckt. No. KRTA-0002-P02), entitled "LASER SCANNER WITH REAL-TIME, ONLINE EGO-MOTION
ESTIMATION," filed on October 11, 2016.
[0003] PCT Application No. PCT/US2017/021120 claims the benefit of U.S.
Provisional Patent Application Serial No. 62/307,061 (Atty. Dckt. No. KRTA-0001-P01), entitled "LASER SCANNER WITH REAL-TIME, ONLINE EGO-MOTION
ESTIMATION," filed on March 11, 2016.
[0004] This application further claims priority to U.S. Provisional No. 62/451,294 (Atty. Dckt. No. KRTA-0004-P01), entitled "LIDAR AND VISION-BASED EGO-MOTION ESTIMATION AND MAPPING," filed January 27, 2017.
[0005] The disclosures of PCT/US2017/055938, PCT/US2017/021120 and U.S. Provisional Nos. 62/406,910, 62/307,061 and 62/451,294 are incorporated herein by reference in their entireties and for all purposes.
BACKGROUND
[0006] An autonomous moving device may require information regarding the terrain in which it operates. Such a device may either rely on a pre-defined map presenting the terrain and any obstacle that may be found therein. Alternatively, the device may have the capabilities to map its terrain while either stationary or in motion comprising a computer-based mapping system with one or more sensors to provide real-time data. The mobile, computer-based mapping system may estimate changes in its position over time (an odometer) and/or generate a three- dimensional map representation, such as a point cloud, of a three-dimensional space.
[0007] Exemplary mapping systems may include a variety of sensors to provide data from which the map may be built. Some mapping systems may use a stereo camera system as one such sensor. These systems benefit from the baseline between the two cameras as a reference to determine scale of the motion estimation. A binocular system is preferred over a monocular system, as a monocular system may not be able to resolve the scale of the image without receiving data from additional sensors or making assumptions about the motion of the device. In recent years, RGB-D cameras have gained popularity in the research community. Such cameras may provide depth information associated with individual pixels and hence can help determine scale. However, some methods including the RGB-D camera may only use the image areas with coverage of depth information, which may result in large image areas being wasted especially in an open environment where depth can only be sparsely available.
[0008] In other examples of mapping systems, an IMU may be coupled one or more cameras with, so that scale constraints may be provided from IMU accelerations. In some examples, a monocular camera may be tightly or loosely coupled to an IMU by means of a Kalman filter. Other mapping systems may use optimization methods to solve for the motion of the mobile system.
[0009] Alternative examples of mapping systems may include the use of laser scanners for motion estimation. However, a difficulty of the use of such data may arise from the scanning rate of the laser. While the system is moving, laser points unlike a fixed position laser scanner are impacted by the relative movement of the scanner. Therefore the impact of this movement may be a factor of the laser points arriving arrive at the system at different times. Consequently, when the scanning rate is slow with respect to the motion of the mapping system, scan distortions may be present due to external motion of the laser. The motion effect can be compensated by a laser itself but the compensation may require an independent motion model to provide the required corrections. As one example, the motion may be modeled as a constant velocity or as a Gaussian process. In some example, an IMU may provide the motion model. Such a method matches spatio-temporal patches formed by laser point clouds to estimate sensor motion and correct IMU biases in off-line batch optimization.
[0010] Similar problems of motion distortion may be found in the use of rolling-shutter cameras. Specifically, image pixels may be received continuously over time, resulting in image distortion caused by extrinsic motion of the camera. In some examples, visual odometry methods may use an IMU to compensate for the rolling-shutter effect given the read-out time of the pixels.
[0011] In some examples, GPS/INS techniques may be used to determine the position of a mobile mapping device. However, high-accuracy GPS/INS solutions may be impractical when the application is GPS-denied, light-weight, or cost-sensitive. It is recognized that accurate GPS mapping requires line-of-sight communication between the GPS receiver and at least four GPS satellites (although five may be preferred). In some environments, it may be difficult to receive undistorted signals from four satellites, for example in urban environments that may include overpasses and other obstructions.
[0012] It may thus be appreciated that there are several technical challenges associated with merging data from optical devices with other motion measuring devices in order to generate a robust map of the terrain surrounding an autonomous mapping device, especially while the mapping device is in motion. Disclosed below are methods and systems of a mapping device capable of acquiring optical mapping information and producing robust maps with reduced distortion.
SUMMARY
[0013] In accordance with an exemplary and non-limiting embodiment, a method comprises receiving data from an IMU device at a first computational module at a first frequency and computing, based at least in part on the received IMU data, a first estimated position of a mobile mapping system, receiving the first estimated position and visual-inertial data at a second computational module at a second frequency and computing, based at least in part on the first estimated position and visual-inertial data, a second estimated position of the mobile mapping system and receiving the second estimated position and laser scan data at a third computational module at a third frequency and computing, based at least in part on the second estimated position and laser scan data, a third estimated position of the mobile mapping system.
[0014] In accordance with another exemplary and non- limiting embodiment, a mobile mapping system comprises a first computational module adapted to receive data from an IMU device at a first frequency and compute, based at least in part on the received IMU data, a first estimated position of the mobile mapping system, a second computational module adapted to receive the first estimated position and visual-inertial data at a second frequency and compute, based at least in part on the first estimated position and visual-inertial data, a second estimated position of the mobile mapping system and a third computational module adapted to receive the second estimated position and laser scan data at a third frequency and compute, based at least in part on the second estimated position and laser scan data, a third estimated position of the mobile mapping system.
BRIEF DESCRIPTION OF THE FIGURES
[0015] FIG. 1 illustrates a block diagram of an embodiment of a mapping system.
[0016] FIG. 2 illustrates an embodiment a block diagram of the three computational modules and their respective feedback features of the mapping system of FIG. 1.
[0017] FIG. 3 illustrates an embodiment of a Kalmann filter model for refining positional information into a map.
[0018] FIG. 4 illustrates an embodiment of a factor graph optimization model for refining positional information into a map.
[0019] FIG. 5 illustrates an embodiment of a visual-inertial odometry subsystem.
[0020] FIG. 6 illustrates an embodiment of a scan matching subsystem.
[0021] FIG. 7A illustrates an embodiment of a large area map having coarse detail resolution.
[0022] FIG. 7B illustrates an embodiment of a small area map having fine detail resolution.
[0023] FIG. 8A illustrates an embodiment of multi-thread scan matching.
[0024] FIG. 8B illustrates an embodiment of single-thread scan matching.
[0025] FIG. 9A illustrates an embodiment of a block diagram of the three computational modules in which feedback data from the visual-inertial odometry unit is suppressed due to data degradation.
[0026] FIG. 9B illustrates an embodiment of the three computational modules in which feedback data from the scan matching unit is suppressed due to data degradation.
[0027] FIG. 10 illustrates an embodiment of the three computational modules in which feedback data from the visual-inertial odometry unit and the scan matching unit are partially suppressed due to data degradation.
[0028] FIG. 11 illustrates an embodiment of estimated trajectories of a mobile mapping device.
[0029] FIG. 12 illustrates bidirectional information flow according to an exemplary and non- limiting embodiment.
[0030] FIGS. 13a and 13b illustrate a dynamically reconfigurable system according to an exemplary and non-limiting embodiment.
[0031] FIG. 14 illustrates priority feedback for IMU bias correction according to an exemplary and non-limiting embodiment. [0032] FIGS. 15a and 15b illustrate a two-layer voxel representation of a map according to an exemplary and non-limiting embodiment.
[0033] FIGS. 16a and 16b illustrate multi-thread processing of scan matching according to an exemplary and non-limiting embodiment.
[0034] FIGS. 17a and 17b illustrate exemplary and non-limiting embodiments of a SLAM system.
[0035] FIG. 18 illustrates an exemplary and non-limiting embodiment of a SLAM enclosure.
[0036] FIGS. 19a, 19b and 19c illustrate exemplary and non- limiting embodiments of a point cloud showing confidence levels.
[0037] FIG. 20 illustrates an exemplary and non-limiting embodiment of differing confidence level metrics.
[0038] FIG. 21 illustrates an exemplary and non-limiting embodiment of a SLAM system.
[0039] FIG. 22 illustrates an exemplary and non-limiting embodiment of timing signals for the SLAM system.
[0040] FIG. 23 illustrates an exemplary and non-limiting embodiment of timing signals for the SLAM system.
[0041] FIG. 24 illustrates an exemplary and non-limiting embodiment of SLAM system signal synchronization.
[0042] FIG. 25 illustrates an exemplary and non-limiting embodiment of air-ground collaborative mapping.
[0043] FIG. 26 illustrates an exemplary and non-limiting embodiment of a sensor pack.
[0044] FIG. 27 illustrates an exemplary and non-limiting embodiment of a block diagram of the laser- visual-inertial odometry and mapping software system.
[0045] FIG. 28 illustrates an exemplary and non-limiting embodiment of a comparison of scans involved in odometry estimation and localization.
[0046] FIG. 29 illustrates an exemplary and non-limiting embodiment of a comparison of scan matching accuracy in localization.
[0047] FIG. 30 illustrates an exemplary and non-limiting embodiment of a horizontally orientated sensor test.
[0048] FIG. 31 illustrates an exemplary and non-limiting embodiment of a vertically orientated sensor test.
[0049] FIG. 32 illustrates an exemplary and non-limiting embodiment of an accuracy comparison between horizontally orientated and downward tilted sensor tests. [0050] FIG. 33 illustrates an exemplary and non-limiting embodiment of an aircraft with a sensor pack.
[0051] FIG. 34 illustrates an exemplary and non-limiting embodiment of sensor trajectories.
[0052] FIG. 35 illustrates an exemplary and non-limiting embodiment of autonomous flight results.
[0053] FIG. 36 illustrates an exemplary and non-limiting embodiment of an autonomous flight result over a long-run.
DETAILED DESCRIPTION
[0054] In one general aspect, the present invention is directed to a mobile, computer-based mapping system that estimates changes in position over time (an odometer) and/or generates a three-dimensional map representation, such as a point cloud, of a three-dimensional space. The mapping system may include, without limitation, a plurality of sensors including an inertial measurement unit (IMU), a camera, and/or a 3D laser scanner. It also may comprise a computer system, having at least one processor, in communication with the plurality of sensors, configured to process the outputs from the sensors in order to estimate the change in position of the system over time and/or generate the map representation of the surrounding environment. The mapping system may enable high-frequency, low-latency, on-line, real-time ego-motion estimation, along with dense, accurate 3D map registration. Embodiments of the present disclosure may include a simultaneous location and mapping (SLAM) system. The SLAM system may include a multi-dimensional (e.g., 3D) laser scanning and range measuring system that is GPS-independent and that provides real-time simultaneous location and mapping. The SLAM system may generate and manage data for a very accurate point cloud that results from reflections of laser scanning from objects in an environment. Movements of any of the points in the point cloud are accurately tracked over time, so that the SLAM system can maintain precise understanding of its location and orientation as it travels through an environment, using the points in the point cloud as reference points for the location.
[0055] In one embodiment, the resolution of the position and motion of the mobile mapping system may be sequentially refined in a series of coarse-to-fine updates. In a non-limiting example, discrete computational modules may be used to update the position and motion of the mobile mapping system from a coarse resolution having a rapid update rate, to a fine resolution having a slower update rate. For example, an DVIU device may provide data to a first computational module to predict a motion or position of the mapping system at a high update rate. A visual-inertial odometry system may provide data to a second computational module to improve the motion or position resolution of the mapping system at a lower update rate. Additionally, a laser scanner may provide data to a third computational, scan matching module to further refine the motion estimates and register maps at a still lower update rate. In one non- limiting example, data from a computational module configured to process fine positional and/or motion resolution data may be fed back to computational modules configured to process more coarse positional and/or motion resolution data. In another non-limiting example, the computational modules may incorporate fault tolerance to address issues of sensor degradation by automatically bypassing computational modules associated with sensors sourcing faulty, erroneous, incomplete, or non-existent data. Thus, the mapping system may operate in the presence of highly dynamic motion as well as in dark, texture-less, and structure-less environments.
[0056] In contrast to existing map-generating techniques, which are mostly off-line batch systems, the mapping system disclosed herein can operate in real-time and generate maps while in motion. This capability offers two practical advantages. First, users are not limited to scanners that are fixed on a tripod or other nonstationary mounting. Instead, the mapping system disclosed herein may be associated with a mobile device, thereby increasing the range of the environment that may be mapped in real-time. Second, the real-time feature can give users feedback for currently mapped areas while data are collected. The online generated maps can also assist robots or other devices for autonomous navigation and obstacle avoidance. In some non-limiting embodiments, such navigation capabilities may be incorporated into the mapping system itself. In alternative non-limiting embodiments, the map data may be provided to additional robots having navigation capabilities that may require an externally sourced map.
[0057] There are several potential applications for the sensor, such as 3D modeling, scene mapping, and environment reasoning. The mapping system can provide point cloud maps for other algorithms that take point clouds as input for further processing. Further, the mapping system can work both indoors and outdoors. Such embodiments do not require external lighting and can operate in darkness. Embodiments that have a camera can handle rapid motion, and can colorize laser point clouds with images from the camera, although external lighting may be required. The SLAM system can build and maintain a point cloud in real time as a user is moving through an environment, such as when walking, biking, driving, flying, and combinations thereof. A map is constructed in real time as the mapper progresses through an environment. The SLAM system can track thousands of features as points. As the mapper moves, the points are tracked to allow estimation of motion. Thus, the SLAM system operates in real time and without dependence on external location technologies, such as GPS. In embodiments, a plurality (in most cases, a very large number) of features of an environment, such as objects, are used as points for triangulation, and the system performs and updates many location and orientation calculations in real time to maintain an accurate, current estimate of position and orientation as the SLAM system moves through an environment. In embodiments, relative motion of features within the environment can be used to differentiate fixed features (such as walls, doors, windows, furniture, fixtures and the like) from moving features (such as people, vehicles, and other moving items), so that the fixed features can be used for position and orientation calculations. Underwater SLAM systems may use blue-green lasers to reduce attenuation.
[0058] The mapping system design follows an observation: drift in egomotion estimation has a lower frequency than a module's own frequency. The three computational modules are therefore arranged in decreasing order of frequency. High-frequency modules are specialized to handle aggressive motion, while low-frequency modules cancel drift for the previous modules. The sequential processing also favors computation: modules in the front take less computation and execute at high frequencies, giving sufficient time to modules in the back for thorough processing. The mapping system is therefore able to achieve a high level of accuracy while running online in real-time.
[0059] Further, the system may be configured to handle sensor degradation. If the camera is non-functional (for example, due to darkness, dramatic lighting changes, or texture-less environments) or if the laser is non-functional (for example due to structure-less environments) the corresponding module may be bypassed and the rest of the system may be staggered to function reliably. Specifically, in some exemplary embodiments, the proposed pipeline automatically determines a degraded subspace in the problem state space, and solves the problem partially in the well-conditioned subspace. Consequently, the final solution is formed by combination of the "healthy" parts from each module. As a result, the resulting combination of modules used to produce an output is neither simply a linear or non-linear combination of module outputs. In some exemplary embodiments, the output reflect a bypass of one or more entire modules in combination with a linear or non-linear combination of the remaining functioning modules. The system was tested through a large number of experiments and results show that it can produce high accuracy over several kilometers of navigation and robustness with respect to environmental degradation and aggressive motion. [0060] The modularized mapping system, disclosed below, is configured to process data from range, vision, and inertial sensors for motion estimation and mapping by using a multi-layer optimization structure. The modularized mapping system may achieve high accuracy, robustness, and low drift by incorporating features which may include:
• an ability to dynamically reconfigure the computational modules;
• an ability to fully or partially bypass failure modes in the computational modules, and combine the data from the remaining modules in a manner to handle sensor and/or sensor data degradation, thereby addressing environmentally induced data degradation and the aggressive motion of the mobile mapping system; and
• an ability to integrate the computational module cooperatively to provide real-time performance.
[0061] Disclosed herein is a mapping system for online ego-motion estimation with data from a 3D laser, a camera, and an IMU. The estimated motion further registers laser points to build a map of the traversed environment. In many real-world applications, ego-motion estimation and mapping must be conducted in real-time. In an autonomous navigation system, the map may be crucial for motion planning and obstacle avoidance, while the motion estimation is important for vehicle control and maneuver.
[0062] FIG 1 depicts a simplified block diagram of a mapping system 100 according to one embodiment of the present invention. Although specific components are disclosed below, such components are presented solely as examples and are not limiting with respect to other, equivalent, or similar components. The illustrated system includes an IMU system 102 such as an Xsens® MTi-30 IMU, a camera system 104 such as an IDS® UI-1220SE monochrome camera, and a laser scanner 106 such as a Velodyne PUCK™ VLP-16 laser scanner. The IMU 102 may provide inertial motion data derived from one or more of an x-y-z accelerometer, a roll-pitch-yaw gyroscope, and a magnetometer, and provide inertial data at a first frequency. In some non-limiting examples, the first frequency may be about 200 Hz. The camera system 104 may have a resolution of about 752x480 pixels, a 76° horizontal field of view (FOV), and a frame capture rate at a second frequency. In some non-limiting examples, the frame capture rate may operate at a second frequency of about 50Hz. The laser scanner 106 may have a 360° horizontal FOV, a 30° vertical FOV, and receive 0.3 million points/second at a third frequency representing the laser spinning rate. In some non-limiting examples, the third frequency may be about 5Hz. As depicted in FIG. 1, the laser scanner 106 may be connected to a motor 108 incorporating an encoder 109 to measure a motor rotation angle. In one non-limiting example, the laser motor encoder 109 may operate with a resolution of about 0.25°·
[0063] The IMU 102, camera 104, laser scanner 106, and laser scanner motor encoder 109 may be in data communication with a computer system 110, which may be any computing device, having one or more processors 134 and associated memory 120, 160, having sufficient processing power and memory for performing the desired odometry and/or mapping. For example, a laptop computer with 2.6GHz i7quad-core processor (2 threads on each core and 8 threads overall) and an integrated GPU memory could be used. In addition, the computer system may have one or more types of primary or dynamic memory 120 such as RAM, and one or more types of secondary or storage memory 160 such as a hard disk or a flash ROM. Although specific computational modules (IMU module 122, visual-inertial odometry module 126, and laser scanning module 132) are disclosed above, it should be recognized that such modules are merely exemplary modules having the functions as described above, and are not limiting. Similarly, the type of computing device 110 disclosed above is merely an example of a type of computing device that may be used with such sensors and for the purposes as disclosed herein, and is in no way limiting.
[0064] As illustrated in FIG.l, the mapping system 100 incorporates a computational model comprising individual computational modules that sequentially recover motion in a coarse-to- fine manner (see also FIG. 2). Starting with motion prediction from an IMU 102 (IMU prediction module 122), a visual-inertial tightly coupled method (visual-inertial odometry module 126) estimates motion and registers laser points locally. Then, a scan matching method (scan matching refinement module 132) further refines the estimated motion. The scan matching refinement module 132 also registers point cloud data 165 to build a map (voxel map 134). The map also may be used by the mapping system as part of an optional navigation system 136. It may be recognized that the navigation system 136 may be included as a computational module within the onboard computer system, the primary memory, or may comprise a separate system entirely.
[0065] It may be recognized that each computational module may process data from one of each of the sensor systems. Thus, the IMU prediction module 122 produces a coarse map from data derived from the IMU system 102, the visual-inertial odometry module 126 processes the more refined data from the camera system 104, and the scan matching refinement module 132 processes the most fine-grained resolution data from the laser scanner 106 and the motor encoder 109. In addition, each of the finer-grained resolution modules further process data presented from a coarser-grained module. The visual-inertial odometry module 126 refines mapping data received from and calculated by the IMU prediction module 122. Similarly, the scan matching refinement module 132, further processes data presented by the visual inertial odometry module 126. As disclosed above, each of the sensor systems acquires data at a different rate. In one non-limiting example, the IMU 102 may update its data acquisition at a rate of about 200 Hz, the camera 104 may update its data acquisition at a rate of about 50 Hz, and the laser scanner 106 may update its data acquisition at a rate of about 5 Hz. These rates are non-limiting and may, for example, reflect the data acquisition rates of the respective sensors. It may be recognized that coarse-grained data may be acquired at a faster rate than more fine-grained data, and the coarse-grained data may also be processed at a faster rate than the fine-grained data. Although specific frequency values for the data acquisition and processing by the various computation modules are disclosed above, neither the absolute frequencies nor their relative frequencies are limiting.
[0066] The mapping and/or navigational data may also be considered to comprise coarse level data and fine level data. Thus, in the primary memory (dynamic memory 120), coarse positional data may be stored in a voxel map 134 that may be accessible by any of the computational modules 122, 126, 132. File detailed mapping data, as point cloud data 165 that may be produced by the scan matching refinement module 132, may be stored via the processor 150 in a secondary memory 160, such as a hard drive, flash drive, or other more permanent memory.
[0067] Not only are coarse-grained data used by the computational modules for more finegrained computations, but both the visual-inertial odometry module 126 and the scan matching refinement module 132 (fine-grade positional information and mapping) can feed back their more refined mapping data to the IMU prediction module 122 via respective feedback paths 128 and 138 as a basis for updating the IMU position prediction. In this manner, coarse positional and mapping data may be sequentially refined in resolution, and the refined resolution data serve as feed-back references for the more coarse resolution computations.
[0068] FIG. 2 depicts a block diagram of the three computational modules along with their respective data paths. The IMU prediction module 122 may receive IMU positional data 223 from the IMU (102, FIG. 1). The visual-inertial odometry module 126 may receive the model data from the IMU prediction module 122 as well as visual data from one or more individually tracked visual features 227a, 227b from the camera (104, FIG. 1). The laser scanner (106, FIG. 1) may produce data related to laser determined landmarks 233a, 233b, which may be supplied to the scan matching refinement module 132 in addition to the positional data supplied by the visual-inertial odometry module 126. The positional estimation model from the visual-inertial odometry module 126 may be fed back 128 to refine the positional model calculated by the IMU prediction module 122. Similarly, the refined map data from the scan matching refinement module 132 may be fed back 138 to provide additional correction to the positional model calculated by the IMU prediction module 122.
[0069] As depicted in FIG. 2, and as disclosed above, the modularized mapping system may sequentially recover and refine motion related data in a coarse-to-fine manner. Additionally, the data processing of each module may be determined by the data acquisition and processing rate of each of the devices sourcing the data to the modules. Starting with motion prediction from an IMU, a visual-inertial tightly coupled method estimates motion and registers laser points locally. Then, a scan matching method further refines the estimated motion. The scan matching refinement module may also register point clouds to build a map. As a result, the mapping system is time optimized to process each refinement phase as data become available.
[0070] Figure 3 illustrates a standard Kalman filter model based on data derived from the same sensor types as depicted in Figure 1. As illustrated in Figure 3, the Kalman filter model updates positional and/or mapping data upon receipt of any data from any of the sensors regardless of the resolution capabilities of the data. Thus, for example, the positional information may be updated using the visual-inertial odometry data at any time such data become available regardless of the state of the positional information estimate based on the IMU data. The Kalman filter model therefore does not take advantage of the relative resolution of each type of measurement. FIG. 3 depicts a block diagram of a standard Kalman filter based method for optimizing positional data. The Kalman filter updates a positional model 322a - 322n sequentially as data are presented. Thus, starting with an initial positional prediction model 322a, the Kalman filter may predict 324a the subsequent positional model 322b. which may be refined based on the receive IMU mechanization data 323. The positional prediction model may be updated 322b in response to the IMU mechanization data 323. in a prediction step 324a followed by update steps seeded with individual visual features or laser landmarks.
[0071] FIG. 4 depicts positional optimization based on a factor-graph method. In this method, a pose of a mobile mapping system at a first time 410 may be updated upon receipt of data to a pose at a second time 420. A factor-graph optimization model combines constraints from all sensors during each refinement calculation. Thus, IMU data 323, feature data 327a, 327b, and similar from the camera, and laser landmark data 333a, 333b, and similar, are all used for each update step. It may be appreciated that such a method increases the computational complexity for each positional refinement step due to the large amount of data required. Further, since the sensors may provide data at independent rates that may differ by orders of magnitude, the entire refinement step is time bound to the data acquisition time for the slowest sensor. As a result, such a model may not be suitable for fast real-time mapping. The modularized system depicted in FIGS. 1 and 2 sequentially recovers motion in a coarse- to-fine manner. In this manner, the degree of motion refinement is determined by the availability of each type of data.
[0072] Assumptions, Coordinates, and Problem
Assumptions and Coordinate Systems
[0073] As depicted above in FIG. 1, a sensor system of a mobile mapping system may include a laser 106, a camera 104, and an IMU 102. The camera may be modeled as a pinhole camera model for which the intrinsic parameters are known. The extrinsic parameters among all of the three sensors may be calibrated. The relative pose between the camera and the laser and the relative pose between the laser and the IMU may be determined according to methods known in the art. A single coordinate system may be used for the camera and the laser. In one non- limiting example, the camera coordinate system may be used, and all laser points may be projected into the camera coordinate system in pre-processing. In one non-limiting example, the IMU coordinate system may be parallel to the camera coordinate system and thus the IMU measurements may be rotationally corrected upon acquisition. The coordinate systems may be defined as follows:
• the camera coordinate system {C} may originate at the camera optical center, in which the jc-axis points to the left, the j-axis points upward, and the z-axis points forward coinciding with the camera principal axis;
• the IMU coordinate system {/} may originate at the IMU measurement center, in which the x-, y-, and z- axes are parallel to {C} and pointing in the same directions; and
• the world coordinate system { W} may be the coordinate system coinciding with {C} at the starting pose.
[0074] In accordance with some exemplary embodiments the landmark positions are not necessarily optimized. As a result, there remain six unknowns in the state space thus keeping computation intensity low. The disclosed method involves laser range measurements to provide precise depth information to features, warranting motion estimation accuracy while further optimizing the features' depth in a bundle. One need only optimize some portion of these measurements as further optimization may result in diminishing returns in certain circumstances.
[0075] In accordance with exemplary and non-limiting embodiments, calibration of the described system may be based, at least in part, on the mechanical geometry of the system. Specifically, the LIDAR may be calibrated relative to the motor shaft using mechanical measurements from the CAD model of the system for geometric relationships between the lidar and the motor shaft. Such calibration as is obtained with reference to the CAD model has been shown to provide high accuracy and drift without the need to perform additional calibration.
MAP Estimation Problem
[0076] A state estimation problem can be formulated as a maximum a posterior (MAP) estimation problem. We may define
Figure imgf000016_0004
as the set of system states
, as the set of control inputs, and , as
Figure imgf000016_0001
Figure imgf000016_0005
the set of landmark measurements. Given the proposed system, Z may be composed of both visual features and laser landmarks. The joint probability of the system is defined as follows,
Figure imgf000016_0002
[0077] where
Figure imgf000016_0006
is a prior of the first system state, P
Figure imgf000016_0010
represents the motion model, and represents the landmark measurement model. For each problem
Figure imgf000016_0007
formulated as (1), there is a corresponding Bayesian belief network representation of the problem. The MAP estimation is to maximize Eq. 1. Under the assumption of zero-mean Gaussian noise, the problem is equivalent to a least-square problem,
Figure imgf000016_0003
[0078] Here,
Figure imgf000016_0008
and are residual errors associated with the motion model and the landmark
Figure imgf000016_0009
measurement model, respectively.
[0079] The standard way of solving Eq. 2 is to combine all sensor data, for example visual features, laser landmarks, and IMU measurements, into a large factor-graph optimization problem. The proposed data processing pipeline, instead, formulates multiple small optimization problems and solves the problems in a coarse-to-fine manner. The optimization problem may be restated as:
Problem : Given data from a laser, a camera, and an IMU, formulate and solve problems as (2) to determine poses of { C} with respect to {W} , then use the estimated poses to register laser points and build a map of the traversed environment in { W].
IMU Prediction Subsystem
IMU Mechanization
[0080] This subsection describes the IMU prediction subsystem. Since the system considers { C} as the fundamental sensor coordinate system, the IMU may also be characterized with respect to {C}. As disclosed above in the sub-section entitled Assumptions and Coordinate Systems, {/} and { C} are parallel coordinate systems. <a(t) and ait) may be two 3 x 1 vectors indicating the angular rates and accelerations, respectively, of {C} at time t. The corresponding biases may be denoted as
Figure imgf000017_0006
and
Figure imgf000017_0007
and and na(t) be the corresponding noises. The
Figure imgf000017_0008
vector, bias, and noise terms are defined in {C}. Additionally, g may be denoted as the constant gravity vector in {W}. The IMU measurement terms are:
Figure imgf000017_0001
where is the rotation matrix from to { C } , and is the translation vector between
Figure imgf000017_0002
Figure imgf000017_0004
Figure imgf000017_0003
{C} and {/}.
[0081] It is noted that the term represents the centrifugal force due to the fact that the
Figure imgf000017_0005
rotation center (origin of {C}) is different from the origin of {/}. Some examples of visual- inertial navigation methods model the motion in {/} to eliminate this centrifugal force term. In the computational method disclosed herein, in which visual features both with and without depth information are used, converting features without depth from {C} to {7} is not straight forward (see below). As a result, the system disclosed herein models all of the motion in {C} instead. Practically, the camera and the IMU are mounted close to each other to maximally reduce effect of the term.
[0082] The IMU biases may be slowly changing variables. Consequently, the most recently updated biases are used for motion integration. First, Eq. 3 is integrated over time. Then, the resulting orientation is used with Eq. 4 for integration over time twice to obtain translation from the acceleration data.
Bias Correction
[0083] The IMU bias correction can be made by feedback from either the camera or the laser (see 128, 138, respectively, in FIGS. 1 and 2). Each feedback term contains the estimated incremental motion over a short amount of time. The biases may be modeled to be constant during the incremental motion. Starting with Eq. 3, may be calculated by comparing the
Figure imgf000017_0009
estimated orientation with IMU integration. The updated ba(t) is used in one more round of integration to re-compute the translation, which is compared with the estimated translation to calculate bait). [0084] To reduce the effect of high-frequency noises, a sliding window is employed keeping a known number of biases. Non-limiting examples of the number of biases used in the sliding window may include 200 to 1000 biases with a recommended number of 400 biases based on a 200Hz IMU rate. A non- limiting example of the number of biases in the sliding window with an IMU rate of 100Hz is 100 to 500 with a typical value of 200 biases. The averaged biases from the sliding window are used. In this implementation, the length of the sliding window functions as a parameter for determining an update rate of the biases. Although alternative methods to model the biases are known in the art, the disclosed implementation is used in order to keep the IMU processing module as a separate and distinct module. The sliding window method may also allow for dynamic reconfiguration of the system. In this manner, the IMU can be coupled with either the camera, the laser, or both camera and laser as required. For example, if the camera is non-functional, the IMU biases may be corrected only by the laser instead.
[0085] To reduce the effect of high-frequency noises, a sliding window may be employed keeping a certain number of biases. In such instances, the averaged biases from the sliding window may be used. In such an implementation, the length of the sliding window functions as a parameter determining an update rate of the biases. In some instances, the biases may be modeled as random walks and the biases updated through a process of optimization.
However, this non-standard implementation is preferred to keep IMU processing in a separate module. The implementation favors dynamic reconfiguration of the system, i.e. the IMU may be coupled with either the camera or the laser. If the camera is non-functional, the IMU biases may be corrected by the laser instead. As described inter-module communication in the sequential modularized system is utilized to fix the IMU biases. This communication enables IMU biases to be corrected.
[0086] In accordance with an exemplary and non-limiting embodiment, IMU bias correction may be accomplished by utilizing feedback from either the camera or the laser. Each of the camera and the laser contains the estimated incremental motion over a short amount of time. When calculating the biases, the methods and systems described herein model the biases to be constant during the incremental motion. Still starting with Eq. 3, by comparing the estimated orientation with IMU integration, the methods and systems described herein can calculate ba (t). The updated l¾(t) is used in one more round of integration to recompute the translation, which is compared with the estimated translation to calculate ba(t).
[0087] In some embodiments, IMU output comprises an angular rate having relatively constant errors over time. The resulting IMU bias is related to the fact that the IMU will always have some difference from ground truth. This bias can change over time. It is relatively constant and not high frequency. The sliding window described above is a specified period of time during which the IMU data is evaluated.
[0088] With reference to Fig. 5, there is provided a system diagram of the visual-inertial odometry subsystem. The method couples vision with an IMU. Both vision and the IMU provide constraints to an optimization problem that estimates incremental motion. At the same time, the method associates depth information to visual features. If a feature is located in an area where laser range measurements are available, depth may be obtained from laser points. Otherwise, depth may be calculated from triangulation using the previously estimated motion sequence. As the last option, the method may also use features without any depth by formulating constraints in a different way. This is true for those features which may not necessarily have laser range coverage or cannot be triangulated due to the fact that they are not necessarily tracked long enough or located in the direction of camera motion. These three alternatives may be used alone or in combination for building the registered point cloud. In some embodiments information is discarded. Eigenvalues and eigenvectors may be computed and used to identify and specify degeneracy in a point cloud. If there is degeneracy in a specific direction in the state space then the solution in that direction in that state space can be discarded.
Visual-inertial Odometry Subsystem
[0089] A block system diagram of the visual-inertial odometry subsystem is depicted in Fig. 5. An optimization module 510 uses pose constraints 512 from the IMU prediction module 520 along with camera constraints 515 based on optical feature data having or lacking depth information for motion estimation 550. A depthmap registration module 545 may include depthmap registration and depth association of the tracked camera features 530 with depth information obtained from the laser points 540. The depthmap registration module 545 may also incorporate motion estimation 550 obtained from a previous calculation. The method tightly couples vision with an IMU. Each provides constraints 512, 515, respectively, to an optimization module 510 that estimates incremental motion 550. At the same time, the method associates depth information to visual features as part of the depthmap registration module 545. If a feature is located in an area where laser range measurements are available, depth is obtained from laser points. Otherwise, depth is calculated from triangulation using the previously estimated motion sequence. As the last option, the method can also use features without any depth by formulating constraints in a different way. This is true for those features which do not have laser range coverage or cannot be triangulated because they are not tracked long enough or located in the direction of camera motion.
Camera Constraints
[0090] The visual-inertial odometry is a key-frame based method. A new key-frame is determined 535 if more than a certain number of features lose tracking or the image overlap is below a certain ratio. Here, right superscript I, I £ Z+ may indicate the last key-frame, and c, c and may indicate the current frame. As disclosed above, the method combines features with and without depth. A feature that is associated with depth at key-frame /, may be denoted as in { C/} . Correspondingly, a feature without depth is denoted as
Figure imgf000020_0005
using normalized coordinates instead. Note that Xi, Xi, xi, and xi are different
Figure imgf000020_0006
from χ and x in Eq. l which represent the system state. Features at key-frames may be associated with depth for two reasons: 1) depth association takes some amount of processing, and computing depth association only at key-frames may reduce computation intensity; and 2) the depthmap may not be available at frame c and thus laser points may not be registered since registration depends on an established depthmap. A normalized feature in {Cc} may be denoted
Figure imgf000020_0003
[0091] Let R; and tf be the 3 x 3 rotation matrix and 3 x 1 translation vector between frames / and c, where R£ G SO(3) and and form an SE(3) transformation. The motion
Figure imgf000020_0008
Figure imgf000020_0009
function between frames / and c may be written as
Figure imgf000020_0001
[0092] has an unknown depth. Let dc be the depth, where
Figure imgf000020_0007
Substituting Xc with and combining the 1st and 2nd rows with the 3rd row in Eq. 5 to eliminate dc, results in
Figure imgf000020_0002
[0093] R(h) and t(h), h £ { 1, 2, 3 }, are the ft-th rows of R; and tf. In the case that depth in unavailable to a feature, let di be the unknown depth at key- frame /. Substituting Xi and Xc with and respectively, and combining all three rows in Eq. 5 to eliminate dk and dc, results in another constraint,
Figure imgf000020_0004
Motion Estimation [0094] The motion estimation process 510 is required to solve an optimization problem combining three sets of constraints: 1) from features with known depth as in Eqs. 6-7; 2) from features with unknown depth as in Eq. 8; and 3) from the IMU prediction 520. may be defined as a 4 x 4 transformation matrix between frames a and b,
Figure imgf000021_0003
[0095] where and are the corresponding rotation matrix and translation vector. Further,
Figure imgf000021_0004
let be a 3 x 1 vector corresponding to R¾ through an exponential map, where
Figure imgf000021_0010
The normalized term 0/11 #11 represents direction of the rotation and II £>ll is the rotation angle. Each corresponds to a set of and containing 6-DOF motion of the camera.
Figure imgf000021_0005
Figure imgf000021_0006
[0096] The solved motion transform between frames / and c - 1, namely may be used to
Figure imgf000021_0009
formulate the IMU pose constraints. A predicted transform between the last two frames c - 1 and c, denoted as ^may be obtained from IMU mechanization. The predicted transform at
Figure imgf000021_0007
frame c is calculated as,
Figure imgf000021_0001
[0097] Let and be the 6-DOF motion corresponding to It may be understood that the
Figure imgf000021_0016
Figure imgf000021_0015
Figure imgf000021_0012
IMU predicted translation, is dependent on the orientation. As an example, the orientation
Figure imgf000021_0014
may determine a projection of the gravity vector through rotation matrix in Eq. 4, and
Figure imgf000021_0013
hence the accelerations that are integrated, may be formulated as a function of 9f, and may
Figure imgf000021_0011
be rewriten as
Figure imgf000021_0008
It may be understood that the 200Hz pose provided by the IMU prediction module 122 (FIGS. 1 and 2) as well as the 50Hz pose provided by the visual-inertial odometry module 126 (FIGS. 1 and 2) are both pose functions. Calculating if (0f) may begin at frame c and the accelerations may be integrated inversely with respect to time. Let Of be the rotation vector corresponding to
Figure imgf000021_0017
in Eq. 5, and θ[ and are the motion to be solved. The
Figure imgf000021_0018
constraints may be expressed as,
Figure imgf000021_0002
in is a relative covariance matrix scaling the pose constraints appropriately with respect to the camera constraints.
[0098] In embodiments of the visual-inertial odometry subsystem, the pose constraints fulfill the motion model and the camera constraints fulfill the landmark measurement model in Eq. 2. The optimization problem may be solved by using the Newton gradient-descent method adapted to a robust fitting framework for outlier feature removal. In this problem, the state space contains Q[ and if. Thus, a full-scale MAP estimation is not performed, but is used only to solve a marginalized problem. The landmark positions are not optimized, and thus only six unknowns in the state space are used, thereby keeping computation intensity low. The method thus involves laser range measurements to provide precise depth information to features, warranting motion estimation accuracy. As a result, further optimization of the features' depth through a bundle adjustment may not be necessary.
Depth Association
[0099] The depthmap registration module 545 registers laser points on a depthmap using previously estimated motion. Laser points 540 within the camera field of view are kept for a certain amount of time. The depthmap is down-sampled to keep a constant density and stored in a 2D KD-tree for fast indexing. In the KD-tree, all laser points are projected onto a unit sphere around the camera center. A point is represented by its two angular coordinates. When associating depth to features, features may be projected onto the sphere. The three closest laser points are found on the sphere for each feature. Then, their validity may be by calculating distances among the three points in Cartesian space. If a distance is larger than a threshold, the chance that the points are from different objects, e.g. a wall and an object in front of the wall, is high and the validity check fails. Finally, the depth is interpolated from the three points assuming a local planar patch in Cartesian space.
[00100] Those features without laser range coverage, if they are tracked over a certain distance and not located in the direction of camera motion, may be triangulated using the image sequences where the features are tracked. In such a procedure, the depth may be updated at each frame based on a Bayesian probabilistic mode.
Scan Matching Subsystem
[00101] This subsystem further refines motion estimates from the previous module by laser scan matching. FIG. 6 depicts a block diagram of the scan matching subsystem. The subsystem receives laser points 540 in a local point cloud and registers them 620 using provided odometry estimation 550. Then, geometric features are detected 640 from the point cloud and matched to the map. The scan matching minimizes the feature-to-map distances, similar to many methods known in the art. However, the odometry estimation 550 also provides pose constraints 612 in the optimization 610. The optimization comprises processing pose constraints with feature correspondences 615 that are found and further processed with laser constraints 617 to produce a device pose 650. This pose 650 is processed through a map registration process 655 that facilitates finding the feature correspondences 615. The implementation uses voxel representation of the map. Further, it can dynamically configure to run on one to multiple CPU threads in parallel.
Laser Constraints
[00102] When receiving laser scans, the method first registers points from a scan 620 into a common coordinate system. m, m e Z+ may be used to indicate the scan number. It is understood that the camera coordinate system may be used for both the camera and the laser. Scan m may be associated with the camera coordinate system at the beginning of the scan, denoted as To locally register 620 the laser points 540, the odometry estimation 550 from
Figure imgf000023_0007
the visual-inertial odometry may be taken as key-points, and the IMU measurements may be used to interpolate in between the key-points.
[00103] Let Pm be the locally registered point cloud from scan m. Two sets of geometric features from Pm may be extracted: one on sharp edges, namely edge points and denoted as em, and the other on local planar surfaces, namely planar points and denoted as Hm. This is through computation of curvature in the local scans. Points whose neighbor points are already selected are avoided such as points on boundaries of occluded regions and points whose local surfaces are close to be parallel to laser beams. These points are likely to contain large noises or change positions over time as the sensor moves.
[00104] The geometric features are then matched to the current map built. Let be
Figure imgf000023_0006
the map point cloud after processing the last scan, is defined in { W} . The points in
Figure imgf000023_0005
are separated into two sets containing edge points and planar points, respectively. Voxels may be used to store the map truncated at a certain distance around the sensor. For each voxel, two 3D KD-trees may be constructed, one for edge points and the other for planar points. Using KD-trees for individual voxels accelerates point searching since given a query point, a specific KD-tree associated with a single voxel needs to be searched (see below).
[00105] When matching scans, m and into { W} are first projected using the best
Figure imgf000023_0003
Figure imgf000023_0004
guess of motion available, then for each point in and
Figure imgf000023_0001
cluster of closest points are
Figure imgf000023_0002
found from the corresponding set on the map. To verify geometric distributions of the point clusters, the associated eigenvalues and eigenvectors may be examined. Specifically, one large and two small eigenvalues indicate an edge line segment, and two large and one small eigenvalues indicate a local planar patch. If the matching is valid, an equation is formulated regarding the distance from a point to the corresponding point cluster,
Figure imgf000024_0010
Figure imgf000024_0003
, indicate the 6-DOF
Figure imgf000024_0001
Motion Estimation
[00106] The scan matching is formulated into an optimization problem 610 minimizing the overall distances described by Eq. 12. The optimization also involves pose constraints 612 from prior motion. Let be the 4 x 4 transformation matrix regarding the pose of {Cm—
Figure imgf000024_0019
i is generated by processing the last scan. Let be the pose transform from
Figure imgf000024_0013
Figure imgf000024_0012
, as provided by the odometry estimation. Similar to Eq. 10, the predicted pose
Figure imgf000024_0011
transform of
Figure imgf000024_0016
in is,
Figure imgf000024_0021
Figure imgf000024_0004
[00107] Let and pose corresponding to and let m be a relative
Figure imgf000024_0014
Figure imgf000024_0015
Figure imgf000024_0017
Figure imgf000024_0018
covariance matrix. The constraints are,
Figure imgf000024_0002
[ ]
[00108] Eq. 14 refers to the case that the prior motion is from the visual-inertial odometry, assuming the camera is functional. Otherwise, the constraints are from the IMU prediction. and may be used to denote the same terms by IMU
Figure imgf000024_0006
Figure imgf000024_0007
mechanization.
Figure imgf000024_0008
is a function of because integration of accelerations is dependent
Figure imgf000024_0020
on the orientation (same with
Figure imgf000024_0009
in Eq. 11). The IMU pose constraints are,
Figure imgf000024_0005
where is the corresponding relative covariance matrix. In the optimization problem, Eqs. 14 and 15 are linearly combined into one set of constraints. The linear combination is determined by working mode of the visual-inertial odometry. The optimization problem refines and , which is solved by the Newton gradient-descent method adapted to a robust fitting framework.
Map in Voxels
[00109] The points on the map are kept in voxels. A 2- level voxel implementation as illustrated in FIGS. 7A and 7B. Mm_x denotes the set of voxels 702, 704 on the first level map 700 after processing the last scan. Voxels 704 surrounding the sensor 706 form a subset Mm_1, denoted as Sm_1. Given a 6-DOF sensor pose, and there is a corresponding Sm_1 which moves with the sensor on the map. When the sensor approaches the boundary of the map, voxels on the opposite side 725 of the boundary are moved over to extend the map boundary 730. Points in moved voxels are cleared resulting in truncation of the map.
[00110] As illustrated in FIG. 7B, each voxel j of the second level map 750
Figure imgf000025_0007
is formed by a set of voxels that are a magnitude smaller, denoted as S m ] _x than those of the first level map 700. Before matching scans, points in
Figure imgf000025_0005
and
Figure imgf000025_0006
are projected onto the map using the best guess of motion, and fill them into Voxels 708 occupied by
Figure imgf000025_0004
points from and are extracted to form Qm-i and stored in 3D KD-trees for scan
Figure imgf000025_0002
Figure imgf000025_0003
matching. Voxels 710 are those not occupied by points from
Figure imgf000025_0008
or K
Figure imgf000025_0009
Upon completion of scan matching, the scan is merged into the voxels 708 with the map. After that, the map points are downsized to maintain a constant density. It may be recognized that each voxel of the first level map 700 corresponds to a volume of space that is larger than a sub-voxel of the second level map 750. Thus, each voxel of the first level map 700 comprises a plurality of sub-voxels in the second level map 750 and can be mapped onto the plurality of sub- voxels in the second level map 750.
[00111] As noted above with respect to FIGS. 7A and 7B, two levels of voxels (first level map 700 and second level map 750) are used to store map information. Voxels corresponding to Mm_1 are used to maintain the first level map 700 and voxels corresponding to m the second level map 750 are used to retrieve the map around the
Figure imgf000025_0001
sensor for scan matching. The map is truncated only when the sensor approaches the map boundary. Thus, if the sensor navigates inside the map, no truncation is needed. Another consideration is that two KD-trees are used for each individual voxel in Sm_i - one for edge points and the other for planar points. As noted above, such a data structure may accelerate point searching. In this manner, searching among multiple KD-trees is avoided as opposed to using two KD-trees for each individual voxel in The later requires more
Figure imgf000025_0010
resources for KD-tree building and maintenance.
[00112] Table 1 compares CPU processing time using different voxel and KD-tree configurations. The time is averaged from multiple datasets collected from different types of environments covering confined and open, structured and vegetated areas. We see that using only one level of voxels, M results in about twice of processing time for KD-tree building
Figure imgf000025_0011
and querying. This is because the second level of voxels, help retrieve the
Figure imgf000026_0001
map precisely. Without these voxel, more points are contained in Q and built into the KD-
Figure imgf000026_0007
trees. Also, by using KD-trees for each voxel, processing time is reduced slightly in comparison to using KD-trees for all voxels in M
[00113] Table 1. Comparison of average CPU processing time on KD-tree operation
Figure imgf000026_0008
Parallel Processing
[00114] The scan matching involves building KD-trees and repetitively finding feature correspondences. The process is time-consuming and takes major computation in the system. While one CPU thread cannot guarantee the desired update frequency, a multi-thread implementation may address issues of complex processing. FIG. 8A illustrates the case where two matcher programs 812, 815 run in parallel. Upon receiving of a scan, a manager program 810 arranges it to match with the latest map available. In one example, composed of a clustered environment with multiple structures and multiple visual features, matching is slow and may not complete before arrival of the next scan. The two matchers 812 and 815 are called alternatively. In one matcher 812 , Pm, 813a Pm-2 , 813b and additional Pin_¾ (for k= an even integer) 813n, are matched with Qm-2 813a, Qm-4 813a , and additional Qm-k (for k=an even integer) 813n, respectively. Similarly, in a second matcher 815, and
Figure imgf000026_0005
additional Pm_fc (for k= an odd integer) 816n, are matched with ( , and
Figure imgf000026_0006
additional Qm-k (for k=an odd integer) 816n, respectively, The use of this interleaving process may provide twice the amount of time for processing. In an alternative example, composed of a clean environment with few structures or visual features, computation is light. In such an example (FIG. 8B), only a single matcher 820 may be called. Because interleaving is not required
Figure imgf000026_0003
are sequentially matched with
Figure imgf000026_0004
respectively (see 827a,
Figure imgf000026_0002
The implementation may be configured to use a maximum of four threads, although typically only two threads may be needed. Transform Integration
[00115] The final motion estimation is integration of outputs from the three modules depicted in FIG. 2. The 5Hz scan matching output produces the most accurate map, while the 50Hz visual-inertial odometry output and the 200Hz IMU prediction are integrated for high- frequency motion estimates.
On Robustness
[00116] The robustness of the system is determined by its ability to handle sensor degradation. The IMU is always assumed to be reliable functioning as the backbone in the system. The camera is sensitive to dramatic lighting changes and may also fail in a dark/texture- less environment or when significant motion blur is present (thereby causing a loss of visual features tracking). The laser cannot handle structure-less environments, for example a scene that is dominant by a single plane. Alternatively, laser data degradation can be caused by sparsity of the data due to aggressive motion. Such aggressive motion comprises highly dynamic motion. As used herein, "highly dynamic motion" refers to substantially abrupt rotational or linear displacement of the system or continuous rotational or translational motion having a substantially large magnitude. The disclosed self-motion determining system may operate in the presence of highly dynamic motion as well as in dark, texture-less, and structureless environments. In some exemplary embodiments, the system may operate while experiencing angular rates of rotation as high as 360 deg per second. In other embodiments, the system may operate at linear velocities up to and including at UOkph. In addition, these motions can be coupled angular and linear motions.
[00117] Both the visual-inertial odometry and the scan matching modules formulate and solve optimization problems according to EQ. 2. When a failure happens, it corresponds to a degraded optimization problem, i.e. constraints in some directions of the problem are ill- conditioned and noise dominates in determining the solution. In one non-limiting method, eigenvalues, denoted as 21, 22, 26, and eigenvectors, denoted as vi, v2, ve, associated with the problem may be computed. Six eigenvalues/eigenvectors are present because the state space of the sensor contains 6-DOF (6 degrees of freedom). Without losing generality, vi, vi, ve may be sorted in decreasing order. Each eigenvalue describes how well the solution is conditioned in the direction of its corresponding eigenvector. By comparing the eigenvalues to a threshold, well-conditioned directions may be separated from degraded directions in the state space. Let h, h — 0; 1, 6, be the number of well-conditioned directions. Two matrices may be defined as:
Figure imgf000028_0002
[00118] When solving an optimization problem, the nonlinear iteration may start with an initial guess. With the sequential pipeline depicted in FIG. 2, the IMU prediction provides the initial guess for the visual-inertial odometry, whose output is taken as the initial guess for the scan matching. For the additional two modules (visual-inertial odometry and scan matching modules), let x be a solution and Ax be an update of x in a nonlinear iteration, in which Δ* is calculated by solving the linearized system equations. During the optimization process, instead of updating x in all directions, x may be updated only in well-conditioned directions, keeping the initial guess in degraded directions instead,
Figure imgf000028_0003
[00119] In Eq. 17, the system solves for motion in a coarse-to-fine order, starting with the IMU prediction, the additional two modules further solving/refining the motion as much as possible. If the problem is well-conditioned, the refinement may include all 6-DOF. Otherwise, if the problem is only partially well-conditioned, the refinement may include 0 to 5-DOF. If the problem is completely degraded, V becomes a zero matrix and the previous module' s output is kept.
[00120] Returning to the pose constraints described in Eqs. 14 and 15, it may be understood that the two equations are linearly combined in the scan matching problem. As defined in Eq. 16, Υμ and denote the matrices containing eigenvectors from the visual- inertial odometry module, V7 represents well-conditioned directions in the subsystem, and v represents degraded directions. The combined constraints are
Figure imgf000028_0004
Figure imgf000028_0001
[00121] In a normal case where the camera is functional, Vv = V7 and Eq.18 is composed of pose constraints from the visual-inertial odometry as in Eq. 14. However, if the camera data are completely degraded, Vv is a zero matrix and Eq. 18 is composed of pose constraints from the IMU prediction according to Eq. 15.
Case Study of Camera Degradation
[00122] As depicted in FIG. 9A, if visual features are insufficiently available for the visual-inertial odometry, the MU prediction 122 bypasses the visual-inertial odometry module 126 fully or partially 924 - denoted by the dotted line -- depending on the number of well- conditioned directions in the visual-inertial odometry problem. The scan matching module 132 may then locally register laser points for the scan matching. The bypassing IMU prediction is subject to drift. The laser feedback 138 compensates for the camera feedback 128 correcting velocity drift and biases of the IMU, only in directions where the camera feedback 128 is unavailable. Thus, the camera feedback has a higher priority, due to the higher frequency making it more suitable when the camera data are not degraded. When sufficient visual features are found, the laser feedback is not used.
Case Study of Laser Degradation
[00123] As shown in FIG. 9B, if environmental structures are insufficient for the scan matching 132 to refine motion estimates, the visual-inertial odometry module 126 output fully or partially bypasses the scan matching module to register laser points on the map 930 as noted by the dotted line. If well-conditioned directions exist in the scan matching problem, the laser feedback contains refined motion estimates in those directions. Otherwise, the laser feedback becomes empty 138.
Case Study of Camera and Laser Degradation
[00124] In a more complex example, both the camera and the laser are degraded at least to some extent. FIG. 10 depicts such an example. A vertical bar with six rows represents a 6- DOF pose where each row is a DOF (degree of freedom), corresponding to an eigenvector in EQ. 16. In this example, the visual-inertial odometry and the scan matching each updates a 3- DOF of motion, leaving the motion unchanged in the other 3-DOF. The IMU prediction 1022a- f may include initial IMU predicted values 1002. The visual-inertial odometry updates 1004 some 3-DOF (1026c, 1026e, 1026f) resulting in a refined prediction 1026a-1026f. The scan matching updates 1006 some 3-DOF (1032b, 1032d, 10321) resulting in a further refined prediction 1032a-1032f. The camera feedback 128 contains camera updates 1028a-1028f and the laser feedback 138 contains laser updates 1038a- 1038f, respectively. In reference to FIG. 10, cells having no shading (1028a, 1028b, 1028d, 1038a, 1038c, 1038e) do not contain any updating information from the respective modules. The total update 1080a- 1080f to the IMU prediction modules is a combination of the updates 1028a-1028f from the camera feedback 128 and the updates 1038a-1038f from the laser feedback 138. In one or more of the degrees of freedom in which feedback is available from both the camera (for example 1028f) and the laser (for example 1038f), the camera updates (for example 1028f) may have priority over the laser updates (for example 1038f). [00125] In practice, however, the visual-inertial odometry module and the scan matching module may execute at different frequencies and each may have its own degraded directions. IMU messages may be used to interpolate between the poses from the scan matching output. In this manner, an incremental motion that is time aligned with the visual-inertial odometry output may be created. Let θ^_1 and t be the 6-DOF motion estimated by the visual-inertial odometry between frames
Figure imgf000030_0009
and be the corresponding terms estimated by the scan matching after time interpolation. and may be the matrices defined in Eq. 16 containing eigenvectors from the visual- inertial odometry module, in which
Figure imgf000030_0021
v represents well-conditioned directions, and
Figure imgf000030_0019
represents degraded directions. Let and and be the same matrices from the scan matching
Figure imgf000030_0022
Figure imgf000030_0023
module. The following equation calculates the combined feedback,
Figure imgf000030_0020
Figure imgf000030_0004
where andj represent the camera and the laser feedback,
Figure imgf000030_0026
Figure imgf000030_0001
[00126] note that only contains solved motion in a subspace of the state space. The
Figure imgf000030_0008
motion from the IMU prediction, namely and may be projected to the null space of
Figure imgf000030_0010
Figure imgf000030_0011
Figure imgf000030_0005
[00127] may be used to denote the IMU predicted
Figure imgf000030_0006
motion formulated as functions of
Figure imgf000030_0015
and
Figure imgf000030_0016
through integration of Eqs. 3 and 4. The orientation
Figure imgf000030_0002
is dependent on both The biases can be calculated by solving the following
Figure imgf000030_0007
equation,
Figure imgf000030_0003
[00128] When the system functions normally, spans the state space, and and
Figure imgf000030_0025
Figure imgf000030_0013
Figure imgf000030_0014
in Eq- 22 are zero matrices. Correspondingly,
Figure imgf000030_0017
and
Figure imgf000030_0018
are calculated from
Figure imgf000030_0024
In a degraded case, the IMU predicted motion, and is used in directions where the
Figure imgf000030_0012
motion is unsolvable (e.g. white row 1080a of the combined feedback in FIG. 10). The result is that the previously calculated biases are kept in these directions.
Experiments
Tests with Scanners
[00129] The odometry and mapping software system was validated on two sensor suites. In a first sensor suite, a Velodyne LIDAR™ HDL-32E laser scanner is attached to a UI- 1220SE monochrome camera and an Xsens® MTi-30 IMU. The laser scanner has 360° horizontal FOV, 40° vertical FOV, and receives 0.7 million points/second at 5Hz spinning rate. The camera is configured at the resolution of 752 x 480 pixels, 76° horizontal FOV, and 50Hz frame rate. The IMU frequency is set at 200Hz. In a second sensor suite, a Velodyne LIDAR™ VLP-16 laser scanner is attached to the same camera and IMU. This laser scanner has 360° horizontal FOV, 30° vertical FOV, and receives 0.3 million points/second at 5Hz spinning rate. Each sensor suite is attached to a vehicle for data collection, which are driven on streets and in off-road terrains, respectively.
[00130] For both sensor suites, a maximum of 300 Harris corners were tracked. To evenly distribute the visual features, an image is separated into 5 x 6 identical sub-regions, each sub-region providing up to 10 features. When a feature loses tracking, a new feature is generated to maintain the feature number in each sub region.
[00131] The software runs on a laptop computer with a 2.6GHz i7 quad-core processor (2 threads on each core and 8 threads overall) and an integrated GPU, in a Linux® system running Robot Operating System (ROS). Two versions of the software were implemented with visual feature tracking running on GPU and CPU, respectively. The processing time is shown in Table 2. The time used by the visual-inertial odometry (126 in FIG. 2) does not vary much with respect to the environment or sensor configuration. For the GPU version, it consumes about 25% of a CPU thread executing at 50Hz. For the CPU version, it takes about 75% of a thread. The sensor first suite results in slightly more processing time than the second sensor suite. This is because the scanner receives more points and the program needs more time to maintain the depthmap and associate depth to the visual features.
[00132] The scan matching (132 in FIG. 2) consumes more processing time which also varies with respect to the environment and sensor configuration. With the first sensor suite, the scan matching takes about 75% of a thread executing at 5Hz if operated in structured environments. In vegetated environments, however, more points are registered on the map and the program typically consumes about 135% of a thread. With the second sensor suite, the scanner receives fewer points. The scan matching module 132 uses about 50-95% of a thread depending on the environment. The time used by the IMU prediction (132 in FIG. 2) is negligible compared to the other two modules.
Accuracy Tests
[00133] Tests were conducted to evaluate accuracy of the proposed system. In these tests, the first sensor suite was used. The sensors were mounted on an off-road vehicle driving around a university campus. After 2.7km of driving within 16 minutes, a campus map was built. The average speed over the test was 2.8m/s.
[00134] Table 2. Average CPU processing time using the first and second sensor suites
Figure imgf000032_0001
[00135] To evaluate motion estimation drift over the test, the estimated trajectory and registered laser points were aligned on a satellite image. Here, laser points on the ground are manually removed. It was determined, by matching the trajectory with streets on the satellite image, that an upper bound of the horizontal error was < 1:0m. It was also determined, by comparing buildings on the same floor, that the vertical error was < 2:0m. This gives an overall relative position drift at the end to be <0:09% of the distance traveled. It may be understood that precision cannot be guaranteed for the measurements, hence only an upper bound of the positional drift was calculated.
[00136] Further, a more comprehensive test was conducted having the same sensors mounted on a passenger vehicle. The passenger vehicle was driven on structured roads for 9.3km of travel. The path traverses vegetated environments, bridges, hilly terrains, and streets with heavy traffic, and finally returns to the starting position. The elevation changes over 70m along the path. Except waiting for traffic lights, the vehicle speed is between 9-18m/s during the test. It was determined that a building found at both the start and the end of the path was registered into two. The two registrations occur because of motion estimation drift over the length of the path. Thus, the first registration corresponds to the vehicle at the start of the test and the second registration corresponds to the vehicle at the end of the test. The gap was measured to be < 20m, resulting in a relative position error at the end of < 0:22% of the distance traveled.
[00137] Each module in the system contributes to the overall accuracy. FIG. 11 depicts estimated trajectories in an accuracy test. A first trajectory plot 1102 of the trajectory of a mobile sensor generated by the visual-inertial odometry system uses the IMU module 122 and the visual-inertial odometry module 126 (see FIG. 2).The configuration used in the first trajectory plot 1102 is similar to that depicted in FIG. 9B. A second trajectory plot 1104 is based on directly forwarding the IMU prediction from the IMU module 122 to the scan matching module, 132 (see FIG. 2) bypassing the visual-inertial odometry. This configuration is similar to that depicted in FIG. 9A. A third trajectory plot 1108 of the complete pipeline is based on the combination of the IMU module 122, the visual inertial odometry module 126, and the scan matching module 132 (see FIG. 2) has the least amount of drift. The position errors of the first two configurations, trajectory plot 1102 and 1104, are about four and two times larger, respectively.
[00138] The first trajectory plot 1102 and the second trajectory plot 1104 can be viewed as the expected system performance when encountering individual sensor degradation. If scan matching is degraded (see FIG. 9B), the system reduces to a mode indicated by the first trajectory plot 1102. If vision is degraded, (see FIG. 9A), the system reduces to a mode indicated by the second trajectory plot 1104. If none of the sensors is degraded, (see FIG 2) the system incorporates all of the optimization functions resulting in the trajectory plot 1108. In another example, the system may take the IMU prediction as the initial guess and but run at the laser frequency (5Hz). The system produces a fourth trajectory plot 1106. The resulting accuracy is only little better in comparison to the second trajectory plot 1104 which uses the IMU directly coupled with the laser, passing the visual-inertial odometry. The result indicates that functionality of the camera is not sufficiently explored if solving the problem with all constraints stacked together.
[00139] Another accuracy test of the system included running mobile sensor at the original lx speed and an accelerated 2x speed. When running at 2x speed, every other data frame for all three sensors is omitted, resulting in much more aggressive motion through the test. The results are listed in Table 3. At each speed, the three configurations were evaluated. At 2x speed, the accuracy of the visual-inertial odometry and the IMU + scan matching configurations reduce significantly, by 0.54% and 0.38% of the distance traveled in comparison to the accuracy at lx speed. However, the complete pipeline reduces accuracy very little, only by 0.04%. The results indicate that the camera and the laser compensate for each other keeping the overall accuracy. This is especially true when the motion is aggressive.
[00140] Table 3. Relative position errors as percentages of the distance traveled
[00141] (Errors at lx speed correspond to the trajectories in Fig. 11)
Figure imgf000034_0001
[00142] With reference to Fig. 12, there is illustrated an exemplary and non-limiting embodiment of bidirectional information flow. As illustrated, three modules comprising an IMU prediction module, a visual-inertial odometry module and a scan-matching refinement module solve the problem step by step from coarse to fine. Data processing flow is from left to right passing the three modules respectively, while feedback flow is from right to left to correct the biases of the IMU.
[00143] With reference to Figs. 13a and 13b, there is illustrated an exemplary and non-limiting embodiment of a dynamically reconfigurable system. As illustrated in Fig. 13a, if visual features are insufficient for the visual-inertial odometry, the IMU prediction (partially) bypasses the visual-inertial odometry module to register laser points locally. On the other hand, if, as illustrated in Fig. 13b, environmental structures are insufficient for the scan matching, the visual-inertial odometry output (partially) bypasses the scan matching refinement module to register laser points on the map.
[00144] With reference to Fig. 14, there is illustrated an exemplary and non- limiting embodiment of priority feedback for IMU bias correction. As illustrated, a vertical bar represents a 6-DOF pose and each row is a DOF. In a degraded case, starting with the IMU prediction on the left where all six rows designated are "IMU", the visual-inertial odometry updates in 3-DOF where the rows become designated "camera", then the scan matching updates in another 3-DOF where the rows turn designated "laser". The camera and the laser feedback is combined as the vertical bar on the left. The camera feedback has a higher priority - "laser" rows from the laser feedback are only filled in if "camera" rows from the camera feedback are not present.
[00145] With reference to Figs. 15a and 15b, there is illustrated an exemplary and non- limiting embodiment of two-layer voxel representation of a map. There is illustrated voxels on the map Mfn_1 (all voxels in Fig. 15a), and voxels surrounding the sensor 5m_1 (dot filled voxels). is a subset of Mm_1. If the sensor approaches the boundary of the map, voxels
Figure imgf000035_0003
on the opposite side of the boundary (bottom row) are moved over to extend the map boundary. Points in moved voxels are cleared and the map is truncated. As illustrated in Fig. 15b each voxel (a dot filled voxel in Fig. 15a) is formed by a set of voxels Sm 1 _1 that are a
Figure imgf000035_0004
magnitude smaller (all voxels in (Fig. Before scan matching, the laser scan may
Figure imgf000035_0002
be projected onto the map using the best guess of motion. Voxels in
Figure imgf000035_0001
occupied by points from the scan are labeled in cross-hatch. Then, map points in cross-hatched voxels are extracted and stored in 3D KD-trees for scan matching.
[00146] With reference to Fig. 16, there is illustrated an exemplary and non-limiting embodiment of multi-thread processing of scan matching. As illustrated, a manager program calls multiple matcher programs running on separate CPU threads and matches scans to the latest map available. Fig. 16a shows a two-thread case. Scans P are matched with
Figure imgf000035_0007
map on each matcher, giving twice amount of time for processing. In
Figure imgf000035_0005
comparison, Fig. 16b shows a one-thread case, where
Figure imgf000035_0006
are matched with Qm, Qm-i, .... The implementation is dynamically configurable using up to four threads.
[00147] In embodiments, a real time SLAM system may be used in combination with a real time navigation system. In embodiments, the SLAM system may be used in combination with an obstacle detection system, such as a LIDAR- or RADAR-based obstacle detection system, a vision-based obstacle detection system, a thermal-based system, or the like. This may include detecting live obstacles, such as people, pets, or the like, such as by motion detection, thermal detection, electrical or magnetic field detection, or other mechanisms..
[00148] In embodiments, the point cloud that is established by scanning the features of an environment may be displayed, such as on a screen forming a part of the SLAM, to show a mapping of a space, which may include mapping of near field features, such as objects providing nearby reflections to the SLAM system, as well as far field features, such as items that can be scanned through spaces between or apertures in the near field features. For example, items in an adjacent hallway may be scanned through a window or door as the mapper moves through the interior of a room, because at different points in the interior of the room different outside elements can be scanned through such spaces or apertures. The resulting point cloud may then comprise comprehensive mapping data of the immediate near field environment and partial mapping of far field elements that are outside the environment. Thus, the SLAM system may include mapping of a space through a "picket fence" effect by identification of far-field pieces through spaces or apertures (i.e., gaps in the fence) in the near field. The far field data may be used to help the system orient the SLAM as the mapper moves from space to space, such as maintaining consistent estimation of location as the mapper moves from a comprehensively mapped space (where orientation and position are well known due to the density of the point cloud) to a sparsely mapped space (such as a new room). As the user moves from the near field to a far field location, the relative density or sparseness of the point cloud can be used by the SLAM system to guide the mapper via, for example, a user interface forming a part of the SLAM, such as directing the mapper to the parts of the far field that could not be seen through the apertures from another space.
[00149] In embodiments, the point cloud map from a SLAM system can be combined with mapping from other inputs such as cameras, sensors, and the like. For example, in a flight or spacecraft example, an airplane, drone, or other airborne mobile platform may already be equipped with other distance measuring and geo-location equipment that can be used as reference data for the SLAM system (such as linking the point cloud resulting from a scan to a GPS-referenced location) or that can take reference data from a scan, such as for displaying additional scan data as an overlay on the output from the other system. For example, conventional camera output can be shown with point cloud data as an overlay, or vice versa.
[00150] In embodiments, the SLAM system can provide a point cloud that includes data indicating the reflective intensity of the return signal from each feature. This reflective intensity can be used to help determine the efficacy of the signal for the system, to determine how features relate to each other, to determine surface IR reflectivity, and the like. In embodiments, the reflective intensity can be used as a basis for manipulating the display of the point cloud in a map. For example, the SLAM system can introduce (automatically, of under user control) some degree of color contrast to highlight the reflectivity of the signal for a given feature, material, structure, or the like. In addition, the system can be married with other systems for augmenting color and reflectance information. In embodiments, one or more of the points in the point cloud may be displayed with a color corresponding to a parameter of the acquired data, such as an intensity parameter, a density parameter, a time parameter and a geospatial location parameter. Colorization of the point cloud may help users understand and analyze elements or features of the environment in which the SLAM system is operating and/or elements of features of the process of acquisition of the point cloud itself. For example, a density parameter, indicating the number of points acquired in a geospatial area, may be used to determine a color that represents areas where many points of data are acquired and another color where data is sparse, perhaps suggesting the presence of artifacts, rather than "real" data. Color may also indicate time, such as progressing through a series of colors as the scan is undertaken, resulting in clear indication of the path by which the SLAM scan was performed. Colorization may also be undertaken for display purposes, such as to provide differentiation among different features (such as items of furniture in a space, as compared to walls), to provide aesthetic effects, to highlight areas of interest (such as highlighting a relevant piece of equipment for attention of a viewer of a scan), and many others.
[00151] In embodiments, the SLAM system can identify "shadows" (areas where the point cloud has relatively few data points from the scan) and can (such as through a user interface) highlight areas that need additional scanning. For example, such areas may blink or be rendered in a particular color in a visual interface of a SLAM system that displays the point cloud until the shadowed area is sufficiently "painted," or covered, by laser scanning. Such an interface may include any indicator (visual, text-based, voice-based, or the like) to the user that highlights areas in the field that have not yet been scanned, and any such indicator may be used to get the attention of the user either directly or through an external device (such as a mobile phone of the user). In accordance with other exemplary and non-limiting embodiments, the system may make reference to external data of data stored on the SLAM, such as previously constructed point clouds, maps, and the like, for comparison with current scan to identify unscanned regions.
[00152] In embodiments, the methods and systems disclosed herein include a SLAM system that provides real-time positioning output at the point of work, without requiring processing or calculation by external systems in order to determine accurate position and orientation information or to generate a map that consists of point cloud data showing features of an environment based on the reflected signals from a laser scan. In embodiments, the methods and systems disclosed herein may also include a SLAM system that provides real time positioning information without requiring post-processing of the data collected from a laser scan.
[00153] In embodiments, a SLAM system may be integrated with various external systems, such as vehicle navigation systems (such as for unmanned aerial vehicles, drones, mobile robots, unmanned underwater vehicles, self-driving vehicles, semi-automatic vehicles, and many others). In embodiments, the SLAM system may be used to allow a vehicle to navigate within its environments, without reliance on external systems like GPS.
[00154] In embodiments, a SLAM system may determine a level of confidence as to its current estimation of position, orientation, or the like. A level of confidence may be based on the density of points that are available in a scan, the orthogonality of points available in a scan, environmental geometries or other factors, or a combination thereof. The level of confidence may be ascribed to position and orientation estimates at each point along the route of a scan, so that segments of the scan can be referenced as low-confidence segments, high-confidence segments, or the like. Low-confidence segments can be highlighted for additional scanning, for use of other techniques (such as making adjustments based on external data), or the like. For example, where a scan is undertaken in a closed loop (where the end point of the scan is the same as the starting point, at a known origin location), any discrepancy between the calculated end location and the starting location may be resolved by preferentially adjusting location estimates for certain segments of the scan to restore consistency of the start- and end- locations. Location and position information in low-confidence segments may be preferentially adjusted as compared to high-confidence segments. Thus, the SLAM system may use confidence-based error correction for closed loop scans.
[00155] In an exemplary and non-limiting embodiment of this confidence-based adjustment, a derivation of the incremental smoothing and mapping (iS AM) algorithm originally developed by Michael Kaess and Frank Dellaert at Georgia Tech ("iSAM: Incremental Smoothing and Mapping" by M. Kaess, A. Ranganathan, and F. Dellaert, IEEE Trans, on Robotics, TRO, vol. 24, no. 6, Dec. 2008, pp. 1365-1378, PDF) may be used. This algorithm processes map data in "segments" and iteratively refines the relative position of those segments to optimize the residual errors of matches between the segments. This enables closing loops by adjusting all data within the closed loop. More segmentation points allows the algorithm to move the data more significantly while fewer segmentation points generates more rigidity.
[00156] If one selects the points for segmentation based on a matching confidence metric, one may utilize this fact to make the map flexible in regions with low confidence and rigid in areas with high confidence so that the loop closure processing does not distribute local errors through sections of the map that are accurate. This can be further enhanced by weighting the segmentation points to assign "flexibility" to each point and distribute error based on this factor.
[00157] In embodiments, confidence measures with respect to areas or segments of a point cloud may be used to guide a user to undertake additional scanning, such as to provide an improved SLAM scan. In embodiments, a confidence measure can be based on a combination of density of points, orthogonality of points and the like, which can be used to guide the user to enable a better scan. It is noted that scan attributes, such as density of points and orthogonality of points, may be determined in real time as the scan progresses. Likewise, the system may sense geometries of the scanning environment that are likely to result in low confidence measures. For example, long hallways with smooth walls may not present any irregularities to differentiate one scan segment from the next. In such instances, the system may assign a lower confidence measure to scan data acquired in such environments. The system can use various inputs such as LIDAR, camera, and perhaps other sensors to determine diminishing confidence and guide the user through a scan with instructions (such as "slow down," "turn left" or the like). In other embodiments, the system may display areas of lower than desired confidence to a user, such as via a user interface, while providing assistance in allowing the user to further scan the area, volume or region of low confidence.
[00158] In embodiments, a SLAM output may be fused with other content, such as outputs from cameras, outputs from other mapping technologies, and the like. In embodiments, a SLAM scan may be conducted along with capture of an audio track, such as via a companion application (optionally a mobile application) that captures time-coded audio notes that correspond to a scan. In embodiments, the SLAM system provides time-coding of data collection during scanning, so that the mapping system can pinpoint when and where the scan took place, including when and where the mapper took audio and/or notes. In embodiments, the time coding can be used to locate the notes in the area of the map where they are relevant, such as by inserting data into a map or scan that can be accessed by a user, such as by clicking on an indicator on the map that audio is available. In embodiments, other media formats may be captured and synchronized with a scan, such as photography, HD video, or the like. These can be accessed separately based on time information, or can be inserted at appropriate places in a map itself based on the time synchronization of the scan output with time information for the other media. In embodiments, a user may use time data to go back in time and see what has changed over time, such as based on multiple scans with different time-encoded data. Scans may be further enhanced with other information, such as date- or time-stamped service record data. Thus, a scan may be part of a multi-dimensional database of a scene or space, where point cloud data is associated with other data or media related to that scene, including time-based data or media. In embodiments, calculations are maintained through a sequence of steps or segments in a manner that allows a scan to be backed up, such as to return to a given point in the scan and re-initiate at that point, rather than having to re-initiate a new scan starting at the origin. This allows use of partial scan information as a starting point for continuing a scan, such as when a problem occurs at a later point in a scan that was initially producing good output. Thus, a user can "unzip" or "rewind" a scan back to a point, and then recommence scanning from that point. The system can maintain accurate position and location information based on the point cloud features and can maintain time information to allow sequencing with other time-based data. Time-based data can also allow editing of a scan or other media to synchronize them, such as where a scan was completed over time intervals and needs to be synchronized with other media that was captured over different time intervals. Data in a point cloud may be tagged with timestamps, so that data with timestamps occurring after a point in time to which a rewind is undertaken can be erased, such that the scan can re-commence from a designated point. In embodiments, a rewind may be undertaken to a point in time and/or to a physical location, such as rewinding to a geospatial coordinate.
[00159] In embodiments, the output from a SLAM-based map can be fused with other content, such as HD video, including by colorizing the point cloud and using it as an overlay. This may include time-synchronization between the SLAM system and other media capture system. Content may be fused with video, still images of a space, a CAD model of a space, audio content captured during a scan, metadata associated with a location, or other data or media.
[00160] In embodiments, a SLAM system may be integrated with other technologies and platforms, such as tools that may be used to manipulate point clouds (e.g., CAD). This may include combining scans with features that are modeled in CAD modeling tools, rapid prototyping systems, 3D printing systems, and other systems that can use point cloud or solid model data. Scans can be provided as inputs to post-processing tools, such as colorization tools. Scans can be provided to mapping tools, such as for adding points of interest, metadata, media content, annotations, navigation information, semantic analysis to distinguish particular shapes and/or identify objects, and the like.
[00161] Outputs can be combined with outputs from other scanning and image-capture systems, such as ground penetrating radar, X-ray imaging, magnetic resonance imaging, computed tomography imaging, thermal imaging, photography, video, SONAR, RADAR, LID AR and the like. This may include integrating outputs of scans with displays for navigation and mapping systems, such as in-vehicle navigation systems, handheld mapping systems, mobile phone navigation systems, and others. Data from scans can be used to provide position and orientation data to other systems, including X, Y and Z position information, as well as pitch, roll and yaw information.
[00162] The data obtained from a real time SLAM system can be used for many different purposes, including for 3D motion capture systems, for acoustics engineering applications, for biomass measurements, for aircraft construction, for archeology, for architecture, engineering and construction, for augmented reality (AR), for autonomous cars, for autonomous mobile robot applications, for cleaning and treatment, for CAD/CAM applications, for construction site management (e.g., for validation of progress), for entertainment, for exploration (space, mining, underwater and the like), for forestry (including for logging and other forestry products like maple sugar management), for franchise management and compliance (e.g., for stores and restaurants), for imaging applications for validation and compliance, for indoor location, for interior design, for inventory checking, for landscape architecture, for mapping industrial spaces for maintenance, for mapping trucking routes, for military/intelligence applications, for mobile mapping, for monitoring oil pipelines and drilling, for property evaluation and other real estate applications, for retail indoor location (such as marrying real time maps to inventory maps, and the like), for security applications, for stockpile monitoring (ore, logs, goods, etc.), for surveying (including doing pre-scans to do better quoting), for UAVs/drones, for mobile robots, for underground mapping, for 3D modeling applications, for virtual reality (including colorizing spaces), for warehouse management, for zoning/planning applications, for autonomous missions, for inspection applications, for docking applications (including spacecraft and watercraft), for cave exploration, and for video games/augmented reality applications, among many others. In all use scenarios, the SLAM system may operate as described herein in the noted areas of use.
[00163] In accordance with an exemplary and non-limiting embodiment, the unit comprises hardware synchronization of the IMU, camera (vision) and the LiDAR sensor. The unit may be operated in darkness or structureless environments for a duration of time. The processing pipeline may be comprised of modules. In darkness, the vision module may be bypassed. In structureless environments, the LiDAR module may be bypassed or partially bypassed. In exemplary and non-limiting embodiments, the IMU, Lidar and camera data are all time stamped and capable of being temporally matched and synchronized. As a result, the system can act in an automated fashion to synchronize image data and point cloud data. In some instances, color data from synchronized camera images may be used to color clod data pixels for display to the user.
[00164] The unit may comprise four CPU threads for scan matching and may run at, for example, 5Hz with Velodyne data. The motion of the unit when operating may be relatively fast. For example, the unit may operate at angular speeds of approximately 360 degree/second and linear speeds of approximately 30m/s.
[00165] The unit may localize to a prior generated map. For example, instead of building a map from scratch, the unit's software may refer to a previously built map and produce sensor poses and a new map within the framework (e.g., geospatial or other coordinates) of the old map. The unit can further extend a map using localization. By developing a new map in the old map frame, the new map can go further on and out of the old map. This enables different modes of use including branching and chaining, in which an initial "backbone" scan is generated first and potentially post-processed to reduce drift and/or other errors before resuming from the map to add local details, such as side rooms in building or increased point density in a region of interest. By taking this approach, the backbone model may be generated with extra care to limit the global drift and the follow-on scans may be generated with the focus on capturing local detail. It is also possible for multiple devices to perform the detailed scanning off of the same base map for faster capture of a large region.
[00166] It is also possible to resume off of a model generated by a different type of device or even generated from a CAD drawing. For example, a higher global accuracy stationary device could build a base map and a mobile scanner could resume from that map and fill in details. In an alternate embodiment, a longer range device may scan the outside and large inside areas of a building and a shorter range device may resume from that scan to add in smaller corridors and rooms and required finer details. Resuming from CAD drawings could have significant advantages for detecting differences between CAD and as-built rapidly.
[00167] Resuming may also provide location registered temporal data. For example, multiple scans may be taken of a construction site over time to see the progress visually. In other embodiments multiple scans of a factory may help with tracking for asset management.
[00168] Resuming may alternately be used to purely provide localization data within the prior map. This may be useful for guiding a robotic vehicle or localizing new sensor data, such as images, thermal maps, acoustics, etc within an existing map.
[00169] In some embodiments, the unit employs relatively high CPU usage in a mapping mode and relatively low CPU usage in a localization mode, suitable for long-time localization/navigation. In some embodiments, the unit supports long-time operations by executing an internal reset every once in a while. This is advantageous as some of the values generated during internal processing increase over time. Over a long period of operation (e.g., a few days), the values may reach a limit, such as a logical or physical limit of storage for the value in a computer, causing the processing, absent a reset, to potentially fail. In some instances, the system may automatically flush RAM memory to improve performance. In other embodiments, the system may selectively down sample older scanned data as might be necessary when performing a real time comparison of newly acquired data with older and/or archived data.
[00170] In other exemplary embodiments, the unit may support a flying application and aerial-ground map merging. In other embodiments, the unit may compute a pose output at the IMU frequency, e.g., 100Hz. In such instances, the software may produce maps as well as sensor poses. The sensor poses tell the sensor position and pointing with respect to the map being developed. High frequency and accurate pose output helps in mobile autonomy because vehicle motion control requires such data. The unit further employs covariance and estimation confidence and may lock a pose when the sensor is static.
[00171] With reference to Figs. 17(a)- 17(b) there is illustrated exemplary and non-limiting embodiments of a SLAM. LIDAR is rotated to create a substantially hemispherical scan. This is performed by a mechanism with a DC motor driving a spur gear reduction to LIDAR mount via a spur gear assembly 1704. The spur gear reduction assembly 1704 enables the LIDAR to be offset from the motor 1708. There is a slip ring in line with the rotation of the LIDAR to provide power and receive data from the spinning LIDAR. An encoder 1706 is also in line with the rotation of the LIDAR to record the orientation of the mechanism during scanning. A thin section contact bearing supports the LIDAR rotation shaft. Counterweights on the LIDAR rotation plate balance the weight about the axis of rotation making the rotation smooth and constant. As depicted in the LIDAR drive mechanism and attachment figures below, the mechanism is designed with minimal slop and backlash to enable maintenance of a constant speed for interpolation of scan point locations. Note that a motor shaft 1710 is in physical communication with a LIDAR connector 1712.
[00172] With reference to Fig. 18, there is illustrated an exemplary and non- limiting embodiment of a SLAM enclosure 1802. The SLAM enclosure 1802 is depicted in a variety of views and perspectives. Dimensions are representative of an embodiment and non- limiting as the size may be similar or different, while maintaining the general character and orientation of the major components, such as the LIDAR, odometry camera, colorization camera, user interface screen, and the like.
[00173] In some embodiments, the unit may employ a neck brace, shoulder brace, carrier, or other wearable element or device (not shown), such as to help an individual hold the unit while walking around. The unit or a supporting element or device may include one or more stabilizing elements to reduce shaking or vibration during the scan. In other embodiments, the unit may employ a remote battery that is carried in a shoulder bag or the like to reduce the weight of the handheld unit, whereby the scanning device has an external power source.
[00174] In other embodiments, the cameras and LIDAR are arranged to maximize a field of view. The camera- laser arrangement poses a tradeoff. On one side, the camera blocks the laser FOV and on the other side, the laser blocks the camera. In such an arrangement, both are blocked slightly but the blocking does not significantly sacrifice the mapping quality. In some embodiments, the camera points in the same direction as the laser because the vision processing is assisted by laser data. Laser range measurements provide depth information to the image features in the processing.
[00175] In some embodiments, there may be employed a confidence metric representing a confidence of spatial data. Such a confidence metric measurement may include, but is not limited to, number of points, distribution of points, orthogonality of points, environmental geometry, and the like. One or more confidence metrics may be computed for laser data processing (e.g., scan matching) and for image processing. With reference to Figs. 19(a)- 19(c), there are illustrated exemplary and non-limiting example images showing point clouds differentiated by laser match estimation confidence. While in practice, such images may be color coded, as illustrated, both the trajectory and the points are rendered as solid or dotted in the cloud based on a last confidence value at the time of recording. In the examples, dark gray is bad and light gray is good. The values are thresholded such that everything with a value >10 is solid. Through experimentation it has been found that with a Velodyne <1 is unreliable, <10 is less reliable, >10 is very good.
[00176] Using these metrics enables automated testing to resolve model issues and offline model correction such as when utilizing a loop-closure tool as discussed elsewhere herein. Use of these metrics further enables alerting the user when matches are bad and possibly auto- pausing, throwing out low confidence data, and alerting the user when scanning. Fig. 19(a) illustrates a scan of a building floor performed at a relatively slow pace. Fig. 19(b) illustrates a scan of the same building floor performed at a relatively quicker pace. Note the prevalence of light fray when compared to the scan acquired from a slower scan pace arising, in part, from the speed at which the scan is conducted. Fig. 19(c) illustrates a display zoomed in on a potential trouble spot of relatively low confidence.
[00177] With reference to Fig. 20, there is illustrated an exemplary and non-limiting embodiment of scan-to-scan match confidence metric processing and an average number of visual features that track between a full laser scan and a map being built from the prior full laser scans may be computed and presented visually. This metric may present useful, but different confidence measures. In Fig. 20, a laser scan confidence metric view is presented in the left frame while an average number of visual features metric is presented in the right frame for the same data. Again, dark gray line indicates lower confidence and/or fewer average number of visual features.
[00178] In some embodiments, there may be employed loop closure. For example, the unit may be operated as one walks around a room, cubicle, in and out of offices, and then back to a starting point. Ideally the mesh of data from the start and end point should mesh exactly. In the reality, there may be some drift. The algorithms described herein greatly minimize such drift. Typical reduction is on the order of lOx versus conventional methods (0.2% v 2%). This ratio reflects the error in distance between the start point and end point divided by the total distance traversed during the loop. In some embodiments, the software recognizes that it is back to a starting point and it can relock to the origin. Once done, one may take the variation and spread it over all of the collected data. In other embodiments, one may lock in certain point cloud data where a confidence metric indicates that the data confidence was poor and one may apply the adjustments to the areas with low confidence.
[00179] In general, the system may employ both explicit and implicit loop closure. In some instances, a user may indicate, such as via a user interface forming a part of the SLAM, that a loop is to be closed. This explicit loop closure may result in the SLAM executing software that operates to match recently scanned data to data acquired at the beginning of the loop in order to snap the beginning and end acquired data together and close the loop. In other embodiments, the system may perform implicit loop closure. In such instances, the system may operate in an automated fashion to recognize that the system is actively rescanning a location that that comprises a point or region of origin for the scan loop.
[00180] In some embodiments, there may be employed confidence-based loop closure. First, one may determine a start and end point of a loop scan of an area that includes multiple segments. Then, one may determine a confidence of a plurality of the multiple segments. One may then make an adjustment to the lower quality segments rather than the higher quality segments in order for the beginning and end of the loop to be coincident.
[00181] In other exemplary embodiments, there may be performed multi-loop confidence- based loop closure. In yet other embodiments, there may be employed semantically adjusted confidence-based loop closure. For example, structural information may be derived from the attribution of a scanned element, i.e., floors are flat, corridors are straight, etc.
[00182] In some instances, there may be employed colorization of LIDAR point cloud data. In some embodiments, coarse colorization may be employed in real time to collected points to help identify what has been captured. In other embodiments, off-line photorealistic colorization may be employed. In other embodiments, each pixel in the camera can be mapped to a unique LIDAR pixel. For example, one may take color data from a pixel in the colorization camera corresponding to LIDAR data in the point cloud, and add the color data to the LIDAR data.
[00183] In accordance with exemplary and non- limiting embodiments, the unit may employ a sequential, multi-layer processing pipeline, solving for motion from coarse to fine. In each layer of the optimization, the prior coarser result is used as an initial guess to the optimization problem. The steps in the pipeline are:
[00184] 1. Start with IMU mechanization for motion prediction, which provides high frequency updates (on order of 200 Hz), but is subject to high levels of drift.
[00185] 2. Then this estimate is refined by a visual-inertial odometry optimization at the frame rate of the cameras (30-40Hz), the optimization problem uses the IMU motion estimate as an initial guess of pose change and adjusts that pose change in an attempt to minimize residual squared errors in motion between several features tracked from the current camera frame to a key frame.
[00186] 3. Then this estimate is further refined by a laser odometry optimization at a lower rate determined by the "scan frame" rate. Scan data comes in continuously, and software segments that data into frames, similar to image frames, at a regular rate, currently that rate is the time it takes for one rotation of the LIDAR rotary mechanism to make each scan frame a full hemisphere of data. That data is stitched together using visual-inertial estimates of position change as the points within the same scan frame are gathered. In the LIDAR odometry pose optimization step the visual odometry estimate is taken as an initial guess and the optimization attempts to reduce residual error in tracked features in the current scan frame matched to the prior scan frame.
[00187] 4. In the final step, the current scan frame is matched to the entire map so far. The laser odometry estimate is taken as the initial guess and the optimization minimizes residual squared errors between features in the current scan frame and features in the map so far.
[00188] The resulting system enables high-frequency, low-latency ego-motion estimation, along with dense, accurate 3D map registration. Further, the system is capable of handling sensor degradation by automatic reconfiguration bypassing failure modules since each step can correct errors in the prior step. Therefore, it can operate in the presence of highly dynamic motion as well as in dark, texture-less, and structure-less environments. During experiments, the system demonstrates 0.22% of relative position drift over 9.3km of navigation and robustness with respect to running, jumping and even highway speed driving (up to 33m/s).
[00189] Other key features of such a system may include:
[00190] Visual feature optimization w/ and w/o depth: The software may attempt to determine a depth of tracked visual features, first by attempting to associate them with the laser data and secondly by attempting to triangulate depth between camera frames. The feature optimization software may then utilize all features with two different error calculations, one for features with depth and one for features without depth. [00191] Laser feature determination: The software may extract laser scan features as the scan line data comes in rather than in the entire scan frame. This is much easier and is done by looking at the smoothness at each point, which is defined by the relative distance between that point and the K nearest points on either side of that point then labeling the smoothest points as planar features and the sharpest as edge features. It also allows for the deletion of some points that may be bad features.
[00192] Map matching and voxelization: A part of how the laser matching works in realtime is how the map and feature data is stored. Tracking the processor load of this stored data is critical to long term scanning and selectively voxelizing, or down-sampling into three- dimensional basic units in order to minimize the data stored while keeping what is needed for accurate matching. Adjusting the voxel sizes, or basic units, of this down-sampling on the fly based on processor load may improve the ability to maintaining real-time performance in large maps.
[00193] Parallel Processing: The software may be setup in such a way that it can utilize parallel processing to maintain real-time performance if data comes in faster than the processor can handle it. This is more relevant with faster point/second LIDARS like the velodyne.
[00194] Robustness: The way the system uses separate optimization steps without including the prior steps estimates as part of the next estimate (aside from being the initial guess) creates some inherent robustness.
[00195] Confidence Metrics: Each optimization step in this process may provide information on the confidence in its own results. In particular, in each step, the following can be evaluated to provide a measure of confidence in results: the remaining residual squared error after the optimization, the number of features tracked between frames, and the like.
[00196] The user may be presented a down scaled, (e.g., sub sampled) version of the multi- spectral model being prepared with data being acquired by the device. In an example, each measured 3cm x 3cm x 3cm cube of model data may be represented in the scaled down version presented on the user interface as a single pixel. The pixel selected for display may be the pixel that is closest to the center of the cube. A representative down-scaled display being generated during operation of the SLAM is shown below. As described, the decision to display a single pixel in a volume represents a binary result indicative of either the presence of one or more points in a point cloud occupying a spatial cube of defined dimensions or the absence of any such points. In other exemplary embodiments, the selected pixel may be attributed, such as with a value indicating the number of pixels inside the defined cube represented by the selected pixel. This attribute may be utilized when displaying the sub sampled point cloud such as by displaying each selected pixel utilizing color and/or intensity to reflect the value of the attribute.
[00197] A visual frame comprises a single 2D color image from the colorization camera. A LIDAR segment comprises s full 360 degree revolution of the LIDAR scanner. The visual frame and LIDAR segment are synchronized so that they can be combined and aligned with the existing model data based on the unit positional data captured from the IMU and related sensors, such as the odometry (e.g., a high speed black/white) camera.
[00198] Problems giving rise to lower confidence metrics include, but are not limited to, reflections off of glossy walls, glass, transparent glass and narrow hallways. In some embodiments, a user of the unit may pause and resume a scan such as by, for example, hitting a pause button and/or requesting a rewind to a point that is a predetermined or requested number of seconds in the past.
[00199] In accordance with an exemplary and non-limiting embodiment, rewinding during a scan may proceed as follows. First, the user of the system indicates a desire to rewind. This may be achieved through the manipulation of user interface forming a part of the SLAM. As a result of indicating a desire to rewind, the system deletes or otherwise removes a portion of scanned data points corresponding to a duration of time. As all scanned data points are time stamped, the system can effectively remove data points after a predetermined time, thus, "rewinding" back to a previous point in a scan. As discussed herein, images from the camera are gathered and time stamped during each scan, As a result, after removing data points after a predetermined point in time, the system may provide the user with a display of an image recorded at the predetermined point in time while displaying the scanned point cloud rewound to the predetermined point in time. The image acts as a guide to help the user of the system reorient the SLAM into a position closely matching the orientation and pose of the SLAM at the previous predetermined point in time. Once the user is oriented close to the previous orientation of the SLAM at the predetermined point in time, the user may indicate a desire to resume scanning such as by engaging a "Go" button on a user interface of the SLAM. In response to the command to resume scanning, the SLAM may proceed execute a processing pipeline utilizing newly scanned data to form an initial estimation of the SLAMs position and orientation. During this process, the SLAM may not add new data to the scan but, rather, may use the newly scanned data to determine and display an instantaneous confidence level of the user s position as well as a visual representation of the extent to which newly acquired data corresponds to the previous scan data. Lastly, once it is established that the SLAM's location and orientation are sufficiently determined with respect to the previously scanned data, scanning may continue.
[00200] As described above, this ability to rewind is enabled, in part, by the data being stored. One may estimate how many points are brought in per second and then estimate how much to "rewind". The unit may inform the user where he was x seconds a go and allow the user to move to that location and take a few scans to confirm that the user is at the appropriate place. For example, the user may be told an approximate place to go to (or the user indicate where they want to restart). If the user is close enough, the unit may figure it out where the user is and tell the user if they are close enough.
[00201] In other exemplary embodiments, the unit may operate in transitions between spaces. For example, If a user walks very quickly through a narrow doorway there may not be enough data and time to determine the user's place in the new space. Specifically, in this example, the boundaries of a door frame may, prior to proceeding through it, block the LIDAR from imaging a portion of the environment beyond the door sufficient to establish a user's location. One option is to detect this lowering of confidence metric and signal to the operator to modify his behavior upon approaching a narrow passage to slow down, such as by a flashing a visual indicator or a changing the color of the screen, and the like.
[00202] With reference to Fig. 21, there is illustrated an exemplary and non- limiting embodiment of a schematic of the SLAM unit 2100. The SLAM unit 2100 may include a timing server to generate multiple signals derived from the IMU's 2106 pulse-per- second (PPS) signal. The generated signals may be used to synchronize the data collected from the different sensors in the unit. A microcontroller 2102 may be used to generate the signals and communicate with the CPU 2104. The quadrature decoder 2108 may either be built into the microcontroller or on an external IC.
[00203] In some exemplary embodiments, the IMU 2206 supplies a rising edge PPS signal that is used to generate the timing pulses for other parts of the system. The camera may receive three signals generated from the IMU PPS signal including one rising edge signal as described above and two falling edge signals, GPIOl (lasting one frame) and GPI02 (lasting two frames as illustrated with reference to Fig. 22.
[00204] As illustrated, each camera receives a trigger signal synchronized with the IMU PPS having a high frame rate of approximately 30 Hz or 40 Hz and a high resolution of approximately 0.5 Hz - 5 Hz.
[00205] Each IMS PPS pulse may zero a counter internal to the microcontroller 2202. The LIDAR' s synchronous output may trigger the following events: [00206] · read the current encoder value through the quadrature decoder, and
[00207] · read the current counter value.
[00208] The encoder and the counter values may be saved together and sent to the CPU. This may happen every 40 Hz, dictated by the LIDAR synchronous output as illustrated with reference to Fig. 23.
[00209] An alternate time synchronization technique may include IMU based pulse-per- second synchronization that facilitates synchronizing the sensors and the computer processor. An exemplary and non-limiting embodiment of this type of synchronization is depicted with reference to Fig. 24.
[00210] The IMU 2400 may be configured to send a Pulse Per Second (PPS) signal 2406 to a LIDAR 2402. Every time a PPS is sent, the computer 2404 is notified by recognizing a flag in the IMU data stream. Then, the computer 2404 follows up and sends a time string to the LIDAR 2402. The LIDAR 2402 synchronizes to the PPS 2406 and encodes time stamps in the LIDAR data stream based on the received time strings.
[00211] Upon receiving the first PPS 2406, the computer 2404 records its system time. Starting from the second PPS, the computer 2404 increases the recorded time by one second, sends the resulting time string to the LIDAR 2402, and then corrects its own system time to track the PPS 2506.
[00212] In this time synchronization scheme, the IMU 2400 functions as the time server, while the initial time is obtained from the computer system time. The IMU 2400 data stream is associated with time stamps based on its own clock, and initialized with the computer system time when the first PPS 2406 is sent. Therefore, the IMU2400, LIDAR 2402, and computer 2404 are all time synchronized. In embodiments, the LIDAR 2402may be a Velodyne LIDAR.
[00213] In accordance with exemplary and non-limiting embodiments, the unit includes a COM express board and a single button interface for scanning.
[00214] In accordance with exemplary and non-limiting embodiments, the process IMU, vision and laser data sensors may be coupled. The unit may work in darkness or structureless environments for long periods of time. In some embodiments, four CPU threads may be employed for scan matching, each running at 5Hz with Velodyne data. As noted above, motion of the unit may be fast and the unit may localize to a prior map and can extend a map using localization. The unit exhibits relatively high CPU usage in mapping mode and relatively low CPU usage in localization mode thus rendering it suitable for long-time.
[00215] In accordance with exemplary and non-limiting embodiments, methods and systems described herein enable collaboration between mapping from the ground and mapping from the air according to the characteristics of each. Ground-based mapping is not necessarily prone to limitations of space or time. Typically, a mapping device carried by a ground vehicle is suitable for mapping in large scale and can move at a high speed. On the other hand, a tight area can be mapped in a hand-held deployment. However, ground-based mapping is limited by the sensor's altitude making it difficult to realize a top-down looking configuration. As illustrated in Fig. 25, the ground-based experiment produces a detailed map of the surroundings of a building, while the roof has to be mapped from the air. If a small aerial vehicle is used, aerial mapping is limited by time due to the short lifespan of batteries. Space also needs to be open enough for aerial vehicles to operate safely.
[00216] The collaborative mapping as described herein may utilize a laser scanner, a camera, and a low-grade IMU to process data through multi-layer optimization. The resulting motion estimates may be at a high rate (~200Hz) with a low drift (typically <0.1% of the distance traveled).
[00217] The high-accuracy processing pipeline described herein may be utilized to merge maps generated from the ground with maps generated from the air in real-time or near realtime. This is achieved, in part, by localization of one output from a ground derived map with respect to an output from an air derived map.
[00218] While the method disclosed herein fulfills collaborative mapping, it further reduces the complexity of aerial deployments. With a ground-based map, flight paths are defined and an aerial vehicle conducts mapping in autonomous missions. In some embodiments, the aerial vehicle is able to accomplish challenging flight tasks autonomously.
[00219] With reference to Fig. 26, there is illustrated an exemplary and non-limiting embodiment of a sensor/computer pack that may be utilized to enable collaborative mapping. The processing software is not necessarily limited to a particular sensor configuration. With reference to Fig. 26(a), the sensor pack 2601 is comprised of a laser scanner 2603 generating 0.3 million points/second, a camera 2605 at 640 x 360 pixels resolution and 50Hz frame rate, and a low-grade IMU 2607 at 200Hz. An onboard computer processes data from the sensors in real-time for ego-motion estimation and mapping. Fig. 26(b) and Fig. 26(c) illustrate the sensor field of view. An overlap is shared by the laser and camera, with which, the processing software associates depth information from the laser to image features as described more fully below.
[00220] In accordance with exemplary and non-limiting embodiments, the software processes data from a range sensor such as a laser scanner, a camera, and an inertial sensor. Instead of combining data from all sensors in a large, full-blown problem, the methods and systems described herein parse the problem as multiple small problems, solve them sequentially in a coarse-to-fine manner. Fig. 27 illustrates a block diagram of the software system. In such a system, modules in the front conduct light processing, ensuring high- frequency motion estimation robust to aggressive motion. Modules in the back take sufficient processing, run at low frequencies to warrant accuracy of the resulting motion estimates and maps.
[00221] The software starts with IMU data processing 2701. This module runs at the IMU frequency to predict the motion based on IMU mechanization. The result is further processed by a visual-inertial coupled module 2703. The module 2703 tracks distinctive image features through the image sequence and solves for the motion in an optimization problem. Here, laser range measurements are registered on a depthmap, with which, depth information is associated to the tracked image features. Since the sensor pack contains a single camera, depth from the laser helps solve scale ambiguity during motion estimation. The estimated motion is used to register laser scans locally. In the third module 2705, these scans are matched to further refine the motion estimates. The matched scans are registered on a map while scans are matched to the map. To accelerate the processing, scan matching utilizes multiple CPU threads in parallel. The map is stored in voxels to accelerate point query during scan matching. Because the motion is estimated at different frequencies, a fourth module 2707 in the system takes these motion estimates for integration. The output holds both high accuracy and low latency beneficial for vehicle control.
[00222] The modularized system also ensures robustness with respect to sensor degradation, by selecting "healthy" modes of the sensors when forming the final solution. For example, when a camera is in a low-light or texture-less environment such as pointing to a clean and white wall, or a laser is in a symmetric or extruded environment such as a long and straight corridor, processing typically fails to generate valid motion estimates. The system may automatically determine a degraded subspace in the problem state space. When degradation happens, the system only solves the problem partially in the well-conditioned subspace of each module. The result is that the "healthy" parts are combined to produce the final, valid motion estimates.
[00223] When a map is available, the method described above can be extended to utilize the map for localization. This is accomplished using a scan matching method. The method extracts two types of geometric features, specifically, points on edges and planar surfaces, based on the curvature in local scans. Feature points are matched to the map. An edge point is matched to an edge line segment, and a planar point is matched to a local planar patch. On the map, the edge line segments and local planar patches are determined by examining the eigenvalues and eigenvectors associated with local point clusters. The map is stored in voxels to accelerate processing. The localization solves an optimization problem minimizing the overall distances between the feature points and their correspondences. Due to the fact that the high accuracy odometry estimation is used to provide initial guess to the localization, the optimization usually converges in 2-3 iterations.
[00224] The localization does not necessarily process individual scans but, rather, stacks a number of scans for batch processing. Thanks to the high-accuracy odometry estimation, scans are registered precisely in a local coordinate frame where drift is negligible over a short period of time (e.g., a few seconds). A comparison is illustrated with reference to Fig. 28 (8.4) where Fig. 28(a) is a single scan that is matched in the previous section (scan matching executes at 5Hz), and Fig. 28(b) shows stacked scans over two seconds, which are matched during localization (scan matching runs at 0.5Hz). One can see the stacked scans contain significantly more structural details, contributing to the localization accuracy and robustness with respect to environmental changes. Additionally, low-frequency execution keeps the CPU usage to be minimal for onboard processing (localization consumes about 10% of a CPU thread). Stacked scans contain significantly more structural details, contributing to the localization accuracy and robustness with respect to environmental changes. Roughly, about 25% of the environment can change or be dynamic and the system will continue to operate well.
[00225] The localization is compared to a particle filter based implementation. The odometry estimation provides the motion model to the particle filter. It uses a number of 50 particles. At each update step, the particles are resampled based on low-variance resampling. Comparison results are shown in Fig. 29 and Table 8.1. Here, errors are defined as the absolute distances from localized scans to the map. During the evaluation, the methods and systems described herein choose a number of planar surfaces and use the distances between points in localized scans to the corresponding planar patches on the map. Fig. 29 illustrates an exemplary and non-limiting embodiment of an error distribution. When running the particle filter at the same frequency as the described method (0.5Hz), the resulting error is five times as large. While in Table 8.1, the CPU processing time is more than double. In another test, running the particle filter at 5Hz helps reduce the error to be slightly larger than the disclosed method. However, the corresponding CPU processing time increases to over 22 times greater. These results imply a particle filter based method does not necessarily take full advantage of the high-accuracy odometry estimation. Table 8.1 Comparison of CPU processing time in localization. When running the particle filter at 5Hz, denser data is processed at 25% of real-time speed due to high CPU demand.
i
|
i
j
Figure imgf000054_0001
[00226] With reference to Fig, 30, there is illustrated an exemplary an non-limiting embodiment of a sensor study wherein the sensor pack is carried horizontally in a garage building. Fig. 30(a) shows the map built and sensor trajectory. Fig. 30(b) is a single scan. In this scenario, the scan contains sufficient structural information. When bypassing the camera processing module, the system produces the same trajectory as the full pipeline. On the other hand, the methods and systems described herein run another test with the sensor pack tilted vertically down toward the ground. The results are shown in Fig. 31. In this scenario, structural information in a scan is much sparser as shown in Fig. 31(b)). The processing fails without usage of the camera and succeeds with the full pipeline. The results indicate the camera is critical for high-altitude flights where tilting of the sensor pack is required.
[00227] With reference to Fig. 32, there is illustrated an exemplary and non-limiting embodiment wherein the sensor pack is held by an operator walking through a circle at l-2m/s speed with an overall traveling distance of 410m. Fig. 32(a) shows the resulting map and sensor trajectory with a horizontally orientated sensor configuration. The sensor is started and stopped at the same position. The test produces 0.18m of drift through the path, resulting in 0.04% of relative position error in comparison to the distance traveled. Then, the operator repeats the path with two sensor packs held at 45° and 90° angles, respectively. The resulting sensor trajectories are shown in Fig. 32(b). Clearly, tilting introduces more drift, where the relative position errors are 0.6% at 45° (blue dash curve) and 1.4% at 90° (red dash-dot curve). Finally, by localizing on the map in Fig. 32(a), the drift is canceled and both configurations result in the black solid curve.
[00228] An exemplary and non-limiting embodiment of a drone platform 3301 is illustrated at Fig. 33. The aircraft weighs approximately 6.8kg (including batteries) and may carry a maximum of 4.2kg payload. The sensor/computer pack is mounted to the bottom of the aircraft, weighting 1.7kg. The bottom right of the figure shows the remote controller. During autonomous missions, the remote controller is operated by a safety pilot to override the autonomy if necessary. Note that the aircraft is built with a GPS receiver (on top of the aircraft). GPS data is not necessarily used in mapping or autonomous.
[00229] In the first collaborative mapping experiment, an operator holds the sensor pack and walks around a building. Results are shown in Fig. 25. In Fig. 25(a), the ground-based mapping covers surroundings of the building in detail, conducted at l-2m/s over 914m of travel. As expected, the roof of the building is empty on the map. Second, the drone is teleoperated to fly over the building. In Fig. 25(b), the flight is conducted at 2-3m/s with a traveling distance of 269m. The processing uses localization w.r.t. the map in Fig. 25(a). That way, the aerial map is merged with the ground-based map (white points). After the ground- based map is built, the take-off position of the drone is determined on the map. The sensor starting pose for the aerial mapping is known, and from which, the localization starts. Fig. 34 presents the aerial and ground-based sensor trajectories, in top-down and side views.
[00230] Further, the methods and systems described herein conduct autonomous flights to realize aerial mapping. With reference to Fig. 35, a ground-based map is built first by handheld mapping at l-2m/s for 672m of travel around the flight area. The map and sensor trajectory are shown in Fig. 35(a). Then based on the map, way-points are defined and the drone follows the way-points to conduct aerial mapping. As shown in Fig. 35(b), the curve is the flight path, the large points on the curve are the way-points, and the points form the aerial map. In this experiment, the drone takes off inside a shed on the left side of the figure, flies across the site and passes through another shed on the right side, then returns to the first shed to land. The speed is 4m/s crossing the site and 2m/s passing through the shed. Fig. 35(c) and Fig. 35(d) are two images taken by an onboard camera when the drone flies toward the shed on the right and is about to enter the shed. Fig. 35(e) shows the estimated speed during the mission.
[00231] Finally, the methods and systems described herein conduct another experiment over a longer distance. As shown in Fig. 36, the ground-based mapping involves an off -road vehicle driven at lOm/s from the left end to the right end, over 1463m of travel. With the ground-based map and way-points, the autonomous flight crosses the site. Upon take-off, the drone ascends to 20m high above the ground at 15m/s. Then, it descends to 2m above the ground to fly through a line of trees at lOm/s. The flight path is 1118m long as indicated by the curve 3601 in Fig. 36(b). Two images are taken as the drone flies high above the trees (see Fig. 36(c)) and low underneath the trees (see Fig. 36(d)).
[00232] In accordance with an exemplary and non-limiting embodiment, global positioning data such as from a GPS may be incorporated into the processing pipeline. Global positioning data can be helpful to cancel ego-motion estimation drift over a long distance of travel and register maps in a global coordinate frame.
[00233] In some embodiments, GPS data may be recorded simultaneously with mapping activity. As the system moves there is some level of drift that causes an error to grow over distance. One may typically experience only 0.2% drift rate but when traveling 1000 meters that is still 2 meters for every 1000 meters of travel. At 10km this grows to 20 meters, etc. Without closing the loop (in the traditional sense of coming back to the beginning of the route) this error cannot be corrected. By providing external information in the form of GPS coordinates, the system can know where it is and correct the current position estimate. While this is typically done in a post-processing effort, the present system is able to accomplish such a correction in real-time or near real-time.
[00234] Thus, utilizing GPS provides a method by which one may close the loop. GPS, of course, has some amount of error as well, but it is usually consistent in a given area and many GPS systems today can provide better than 30cm accuracy in position X and Y on the surface. Other more expensive and sophisticated systems can provide cm level positioning.
[00235] This use of GPS provides important capabilities: 1. The location of the point cloud on the planet. 2. the ability to use the course-corrected information to align and "fix" the map so that the map becomes even more accurate since one knows one's position and any data taken at that position may now be referenced to the series of GPS points that are also collected. 3. The ability for the system to act as an IMU when GPS is lost.
[00236] In accordance with an exemplary and non-limiting embodiment, dynamic vision sensors may be utilized to further improve estimation robustness with respect to aggressive motion. A dynamic vision sensor reports data only on pixels with illumination changes, delivering both a high rate and a low latency.
[00237] This high rate (typically defined as more than approximately 10Hz) may provide rapid information quickly to the ego-motion and estimation system thus improving values for localization and, subsequently, mapping. If the system is able to capture more data with fewer delays, the system will be more accurate, and more robust. The features that are identified and tracked by the dynamic vision sensor enable better estimates since more features, and faster updates enable more accurate tracking and motion estimation.
[00238] Direct methods may be used to realize image matching with a dynamic vision sensor for ego-motion estimation. Specifically, direct methods match sequential images for feature tracking from image to image. In contrast, the feature tracking method disclosed herein is superior to the direct method.
[00239] In accordance with an exemplary and non-limiting embodiment, parallel processing may be implemented to execute on a general purpose GPU or FPGA and therefore enable data processing in larger amount and higher frequencies.
[00240] Using parallel processing techniques, a number of feature may be tracked simultaneously or data in defined areas or directions may be partitioned into multiple smaller problems. Parallel architectures may take the form of multiple cores, thread, processors or even specialized forms such as Graphics Processing Units (GPUs). By employing a divide and conquer approach, there may be a significant speed-up in overall processing as each node in the parallel architecture works on its portion of the problem at the same time. Typically such speed-ups are not linear. That is, by dividing the problem into 16 parts and processing each part separately does not necessarily speed up the processing by 16 times. There is overhead in making the division, communications, and then re-assembling the results.
[00241] In contrast, with linear processing, each sub-problem or calculation is worked on once and data is shuttled in and out of memory, the CPU, and then back into memory or storage. This is slower although a pipeline architecture, if set up correctly, such as in systolic arrays can improve things as long as the pipeline is kept filled.
[00242] In accordance with an exemplary and non-limiting embodiment, loop closures may be introduced to remove ego-motion estimation drift by global smoothing.
[00243] For example, when you return to your starting point you know that the position estimate can be corrected since you were there already. The starting point is your origin and there must have been an accumulated error, which, at the end of the traverse, becomes evident. By taking the difference between the origin (0,0,0) and the position you think you are at when you return to the beginning (e.g. (1,2,3)) you now know that error value. But the value accumulated, perhaps evenly, over the traverse.
[00244] For example, if you traveled 100 meters and returned to your starting point and found you were off by 1 meter in xyz, you now have the problem of determining where to apply that error over the whole distance. One way would be to spread that error over the entire distance then you might correct the travel by 1cm at each meter increment to stay on the path and, importantly, be back where you started with no error. This is global smoothing.
[00245] Another way may be to use some other measure of error during the traverse and applying corrections for only those places where the measure of error is high. For example, the covariance matrix provides a convenient metric for map quality and may be used to proactively distribute this error over the full traverse. In other words, locations with low-quality matching that show up in the covariance matrix could be used to unevenly spread the error across the traverse in accordance with the proportion of the error over that traverse. This will act to correct the error in the precise areas where low-quality values were associated with particular locations.
[00246] The following clauses provide additional statements regarding embodiments as disclosed herein.
[00247] Clause 1. A method comprising: acquiring a LIDAR point cloud comprising a plurality of points each of which are attributed with at least a geospatial coordinate and a segment, assigning a confidence level to each segment indicative of a computed accuracy of the plurality of points attributed with the same segment and adjusting the geospatial coordinate of each of at least a portion of the plurality of points attributed with the same segment based, at least in part, on a confidence level.
[00248] Clause 2. The method of clause 1, wherein the confidence level is a confidence level of loop closure.
[00249] Clause 3. The method of clause 1, wherein adjusting the geospatial coordinate is in response to a determined loop closure error.
[00250] Clause 4. The method of clause 1, wherein a segment for adjusting the geospatial coordinate has a lower confidence level than at least one other segment.
[00251] Clause 5. A method comprising: commencing to acquire a LIDAR point cloud with a SLAM the point cloud having a starting location and comprising a plurality of points each of which are attributed with at least a geospatial coordinate and a segment, traversing a loop while acquiring the LIDAR point cloud and determining a scan end point when the SLAM is in proximity to the starting location.
[00252] Clause 6. The method of clause 6, wherein determining the scan end point comprises receiving an indication from a user of the SLAM that the loop has been traversed.
[00253] Clause 7. The method of clause 6, wherein determining the scan end point is based on points in a segment exhibiting a proximity to the starting location.
[00254] Clause 8. The method of clause 6, wherein points in a segment other than a segment comprising the starting location are attributed with a geospatial coordinate that is proximal to the starting location.
[00255] Clause 9. A method comprising: acquiring a LIDAR point cloud comprising a plurality of points each of which are attributed with at least a geospatial coordinate and a timestamp, acquiring color image data comprising a plurality of images each of which are attributed with at least one of the geospatial coordinates and the timestamp and colorizing at least a portion of the plurality of points with color information derived from an image having a timestamp that is close in time to the timestamp of each point being colorized and having a geospatial coordinate that is close in proximity to the geospatial coordinate of the colorized plurality of points.
[00256] Clause 10. The method of clause 9, wherein the colorizing is performed in one of real time and near real time.
[00257] Clause 11. A method comprising: deriving a motion estimate for a SLAM system using an IMU forming a part of the SLAM system, refining the motion estimate via a visual- inertial odometry optimization process to produce a refined estimate and refining the refined estimate via a laser odometry optimization process by minimizing at least one residual squared error between at least one feature in a current scan and at least one previously scanned feature.
[00258] Clause 12. The method of clause 11, wherein deriving the motion estimate comprises receiving IMU updates at a frequency of approximately 200 Hz,
[00259] Clause 13. The method of clause 12, wherein a frequency of the IMU updates are between 190 Hz and 210 Hz.
[00260] Clause 14. The method of clause 11, wherein refining the motion estimate comprises refining the motion estimate at a rate equal to a frame rate of a camera forming a part of the SLAM system.
[00261] Clause 15. The method of clause 14, wherein the frame rate is between 30 Hz and 40 Hz.
[00262] Clause 16. The method of clause 11, wherein the laser odometry optimization process is performed at a scan frame rate at which a LIDAR rotary mechanism forming a part of the SLAM scans a full hemisphere of data.
[00263] Clause 17. A method comprising: acquiring a plurality of depth tracked visual features in a plurality of camera frames using a camera forming a part of a SLAM system, associating the plurality of visual features with a LIDAR derived point cloud acquired from a LIDAR forming a part of the SLAM and triangulating a depth of at least one visual feature between at least two camera frames.
[00264] Clause 18. The method of clause 17, where in the associating and triangulating steps are performed on a processor employing parallel computing.
[00265] Clause 19. A SLAM device comprising: a microcontroller, an inertial measurement unit (IMU) adapted to produce a plurality of timing signals and a timing server adapted to generate a plurality of synchronization signals derived from the plurality of timing signals, wherein the synchronization signals operate to synchronize at least two sensors forming a part of the SLAM device. [00266] Clause 20. The SLAM device of clause 20, wherein the at least two sensors are selected from the group consisting of LIDAR, a camera and an IMU.
[00267] Clause 21. A method comprising: acquiring a LIDAR point cloud comprising a plurality of points each of which are attributed with at least a geospatial coordinate and a timestamp, acquiring color image data comprising a plurality of images each of which are attributed with at least the geospatial coordinate and the timestamp, colorizing at least a portion of the plurality of points with color information derived from an image having at least one of a timestamp that is close in time to the timestamp of each point being colorized and a geospatial coordinate that is close in distance to the geospatial coordinate of each point being colorized and displaying the colorized portion of the plurality of points.
[00268] Clause 22. The method of clause 21, further comprising displaying output from a camera as an overlay on the displayed plurality of points.
[00269] Clause 23. A method comprising acquiring a LIDAR point cloud comprising a plurality of points each of which are attributed with at least a geospatial coordinate and a timestamp and colorizing at least a portion of the plurality of points with color information, wherein each of the plurality of points is colorized with a color corresponding to a parameter of the acquired LIDAR point cloud data selected from the group consisting of an intensity parameter, a density parameter, a time parameter and a geospatial location parameter.
[00270] Clause 24. A method comprising: acquiring a LIDAR point cloud with a SLAM comprising a plurality of near field points derived from a corresponding near environment and a plurality of far field points derived from a corresponding far field environment wherein the far field points are scanned through one or more spaces between one or more elements located in the near environment and utilizing the plurality of far field points to orient the SLAM as it moves from the near environment to the far environment.
[00271] Clause 25. A method comprising receiving feedback comprising a plurality of feedback terms at a SLAM system from at least one of a camera and a laser and modeling a plurality of biases each associated with one of the plurality of feedback terms wherein the plurality of biases form a sliding window of biases.
[00272] Clause 26. The method of clause 25, wherein each of the plurality of feedback terms comprises an estimated incremental motion of the SLAM system.
[00273] Clause 27. The method of clause 26, wherein each of the plurality of biases term comprises is modeled to be constant during the incremental motion.
[00274] Clause 28. The method of clause 25, wherein the sliding window comprises between 200 and 1000 biases. [00275] Clause 29. The method of clause 28, wherein the sliding window comprises approximately 400 biases.
[00276] Clause 30. The method of clause 25, wherein a length of the sliding window functions as a parameter for determining an update rate of the plurality of biases.
[00277] Clause 31. The method of clause 25, wherein the sliding window is adapted to enable dynamic reconfiguration of the SLAM system.
[00278] Clause 32. The method of clause 25, further comprising performing IMU bias correction on the plurality of biases.
[00279] Clause 33. The method of clause 32, wherein performing IMU bias correction comprises utilizing data from at least one of a laser and a camera.
[00280] Clause 34. The method of clause 33 wherein the laser and the camera form a part of the SLAM system.
[00281] Clause 35. The method of clause 33, wherein each of the laser and the camera contain an estimated incremental motion over time.
[00282] Clause 36. The method of clause 25, wherein the sliding window is an array formed of a predetermined number of biases and wherein biases are added and removed in a first- in/first-out manner.
[00283] Clause 37. A method comprising receiving vision data from a camera and inertial data from an IMU the camera and the IMU forming a part of a SLAM system, estimating incremental motion of the SLAM system using the vision data and inertial data as constraints and associating depth information with one or more visual features derived from the vision data.
[00284] Clause 38. The method of clause 37, wherein the depth information is obtained from laser data.
[00285] Clause 39. The method of clause 38, wherein, if laser data is unavailable, depth information may be calculated from triangulation using the estimated incremental motion.
[00286] Clause 40. The method of clause 37, wherein the depth information is utilized to build a registered point cloud.
[00287] Clause 41. The method of clause 40, further comprising computing one or more eigenvectors for the point cloud and using the one or more eigenvectors to specify a degeneracy of the point cloud.
[00288] Clause 42. The method of clause 41, wherein degeneracy in a direction of a state space as indicated by at least one of the eigenvectors is utilized to discard a solution in the direction. [00289] Clause 43. A method comprising determining a relative pose between a camera having a camera coordinate system and a laser having a laser coordinate system both forming a part of a SLAM system by utilizing a single coordinate system for the camera and the laser and determining a relative pose between the laser and an IMU forming a part of a SLAM system and having an IMU coordinate system.
[00290] Clause 44. The method of clause 43, wherein utilizing the single coordinate system further comprises projecting a plurality of laser points into the camera coordinate system during pre-processing.
[00291] Clause 45. The method of clause 43, wherein IMU coordinate system is approximately parallel to the camera coordinate system.
[00292] Clause 46. The method of clause 45, further comprising rotationally correcting IMU data from the IMU upon an acquisition of the IMU data.
[00293] Clause 47. The method of clause 43 wherein the camera coordinate system originates at a camera optical center in which an x-axis points to the left, a _y-axis points upward, and a z-axis points forward coinciding with a camera principal axis.
[00294] Clause 48. The system of clause 43, wherein the IMU coordinate system originates at an IMU measurement center, in which x-, y-, and z- axes are parallel to and pointing in the same directions.
[00295] Clause 49. The system of clause 43, wherein a world coordinate system is the coordinate system coinciding with a the starting pose of the SLAM system.
[00296] Clause 50. A method comprising establishing a motion model of a mobile mapping system utilizing one or more pose constraints, establishing a landmark measurement model of a mobile mapping system the landmark measurement model comprising one or more landmark positions utilizing one or more camera constraints and solving for each of the motion model and landmark measurements.
[00297] Clause 51. The method of clause 50, wherein the solving of each model comprises utilizing a Newton gradient-descent method.
[00298] Clause 52. The method of clause 51 , wherein the Newton gradient-descent method adapted to a robust fitting framework for outlier feature removal.
[00299] Clause 53. The method of clause 50, wherein the solving in performed without optimizing the plurality of landmark positions.
[00300] Clause 54. A method comprising receiving an odometry estimation from a visual- inertial odometry module of a mobile mapping system comprising a plurality of key points, receiving a plurality of IMU measurements from an IMU forming a part of the mobile mapping system and registering a plurality of laser points gathered from a laser forming a part of the mobile mapping system based, at least in part, on the IMU measurements.
[00301] Clause 55. The method of clause 54, wherein the registering comprises utilizing IMU measurements to interpolate between a plurality of key points.
[00302] Clause 56. The method of clause 55, wherein the interpolating comprises selecting one or more geometric features from a point cloud formed of the key points used for tracking.
[00303] Clause 57. The method of clause 56, wherein bad points in the point cloud are not selected based upon a geometry.
[00304] Clause 58. The method of clause 56, wherein bad points in the point cloud are not selected based upon a relationship with one or more points and surfaces in the point cloud.
[00305] Clause 59. A method comprising storing map information comprising a point cloud in a plurality of first level voxels each occupying an identically sized first volume, storing the map information in a plurality of second level voxels each occupying an identically sized second volume wherein the first volume that is larger than the second volume, retrieving map information from the second level voxels in proximity to a laser scanner of a mobile mapping system, performing scan matching on the retrieved second level voxels and maintaining the map information comprised of first level voxels using the first level voxels.
[00306] Clause 60. The method of clause 59, where in each first level voxel is mapped to a plurality of second level voxels.
[00307] Clause 61. The method of clause 59, wherein the second level voxels are stored in a 3D KD-tree.
[00308] Clause 62. The method of claim 59, further comprising downsizing the point cloud to maintain a near constant point density.
[00309] Clause 63. A method comprising providing a ground generated point cloud map, generating an air generated point cloud map and merging in real or near-real time the ground generated point cloud map and the air generated point cloud map.
[00310] Clause 64. The method of clause 63, wherein the air generated point cloud map is generated utilizing a drone comprising a mobile mapping system.
[00311] Clause 65. The method of clause 63, wherein the drone comprises a laser scanner, a camera and a low-grade IMU.
[00312] Clause 66. The method of clause 65, wherein data from the laser scanner, the camera and the IMU is processed via a multi-layer optimization process. [00313] Clause 67. The method of clause 63, wherein the merging further comprises localizing at least one output from the ground derived map with respect to an output from the air generated point cloud map.
[00314] Clause 68. The method of clause 64, wherein generating the air generated point cloud map comprises defining a drone flight path and autonomously flying the drone in accordance with the drone flight path.
[00315] While only a few embodiments of the present disclosure have been shown and described, it will be obvious to those skilled in the art that many changes and modifications may be made thereunto without departing from the spirit and scope of the present disclosure as described in the following claims. All patent applications and patents, both foreign and domestic, and all other publications referenced herein are incorporated herein in their entireties to the full extent permitted by law.
[00316] The methods and systems described herein may be deployed in part or in whole through a machine that executes computer software, program codes, and/or instructions on a processor. The present disclosure may be implemented as a method on the machine, as a system or apparatus as part of or in relation to the machine, or as a computer program product embodied in a computer readable medium executing on one or more of the machines. In embodiments, the processor may be part of a server, cloud server, client, network infrastructure, mobile computing platform, stationary computing platform, or other computing platform. A processor may be any kind of computational or processing device capable of executing program instructions, codes, binary instructions and the like. The processor may be or may include a signal processor, digital processor, embedded processor, microprocessor or any variant such as a co-processor (math co-processor, graphic co-processor, communication co-processor and the like) and the like that may directly or indirectly facilitate execution of program code or program instructions stored thereon. In addition, the processor may enable execution of multiple programs, threads, and codes. The threads may be executed simultaneously to enhance the performance of the processor and to facilitate simultaneous operations of the application. By way of implementation, methods, program codes, program instructions and the like described herein may be implemented in one or more thread. The thread may spawn other threads that may have assigned priorities associated with them; the processor may execute these threads based on priority or any other order based on instructions provided in the program code. The processor, or any machine utilizing one, may include non- transitory memory that stores methods, codes, instructions and programs as described herein and elsewhere. The processor may access a non-transitory storage medium through an interface that may store methods, codes, and instructions as described herein and elsewhere. The storage medium associated with the processor for storing methods, programs, codes, program instructions or other type of instructions capable of being executed by the computing or processing device may include but may not be limited to one or more of a CD-ROM, DVD, memory, hard disk, flash drive, RAM, ROM, cache and the like.
[00317] A processor may include one or more cores that may enhance speed and performance of a multiprocessor. In embodiments, the process may be a dual core processor, quad core processors, other chip-level multiprocessor and the like that combine two or more independent cores (called a die).
[00318] The methods and systems described herein may be deployed in part or in whole through a machine that executes computer software on a server, client, firewall, gateway, hub, router, or other such computer and/or networking hardware. The software program may be associated with a server that may include a file server, print server, domain server, internet server, intranet server, cloud server, and other variants such as secondary server, host server, distributed server and the like. The server may include one or more of memories, processors, computer readable media, storage media, ports (physical and virtual), communication devices, and interfaces capable of accessing other servers, clients, machines, and devices through a wired or a wireless medium, and the like. The methods, programs, or codes as described herein and elsewhere may be executed by the server. In addition, other devices required for execution of methods as described in this application may be considered as a part of the infrastructure associated with the server.
[00319] The server may provide an interface to other devices including, without limitation, clients, other servers, printers, database servers, print servers, file servers, communication servers, distributed servers, social networks, and the like. Additionally, this coupling and/or connection may facilitate remote execution of program across the network. The networking of some or all of these devices may facilitate parallel processing of a program or method at one or more location without deviating from the scope of the disclosure. In addition, any of the devices attached to the server through an interface may include at least one storage medium capable of storing methods, programs, code and/or instructions. A central repository may provide program instructions to be executed on different devices. In this implementation, the remote repository may act as a storage medium for program code, instructions, and programs.
[00320] The software program may be associated with a client that may include a file client, print client, domain client, internet client, intranet client and other variants such as secondary client, host client, distributed client and the like. The client may include one or more of memories, processors, computer readable media, storage media, ports (physical and virtual), communication devices, and interfaces capable of accessing other clients, servers, machines, and devices through a wired or a wireless medium, and the like. The methods, programs, or codes as described herein and elsewhere may be executed by the client. In addition, other devices required for execution of methods as described in this application may be considered as a part of the infrastructure associated with the client.
[00321] The client may provide an interface to other devices including, without limitation, servers, other clients, printers, database servers, print servers, file servers, communication servers, distributed servers and the like. Additionally, this coupling and/or connection may facilitate remote execution of program across the network. The networking of some or all of these devices may facilitate parallel processing of a program or method at one or more location without deviating from the scope of the disclosure. In addition, any of the devices attached to the client through an interface may include at least one storage medium capable of storing methods, programs, applications, code and/or instructions. A central repository may provide program instructions to be executed on different devices. In this implementation, the remote repository may act as a storage medium for program code, instructions, and programs.
[00322] The methods and systems described herein may be deployed in part or in whole through network infrastructures. The network infrastructure may include elements such as computing devices, servers, routers, hubs, firewalls, clients, personal computers, communication devices, routing devices and other active and passive devices, modules and/or components as known in the art. The computing and/or non-computing device(s) associated with the network infrastructure may include, apart from other components, a storage medium such as flash memory, buffer, stack, RAM, ROM and the like. The processes, methods, program codes, instructions described herein and elsewhere may be executed by one or more of the network infrastructural elements. The methods and systems described herein may be adapted for use with any kind of private, community, or hybrid cloud computing network or cloud computing environment, including those which involve features of software as a service (SaaS), platform as a service (PaaS), and/or infrastructure as a service (IaaS).
[00323] The methods, program codes, and instructions described herein and elsewhere may be implemented on a cellular network has sender-controlled contact media content item multiple cells. The cellular network may either be frequency division multiple access (FDMA) network or code division multiple access (CDMA) network. The cellular network may include mobile devices, cell sites, base stations, repeaters, antennas, towers, and the like. The cell network may be a GSM, GPRS, 3G, EVDO, mesh, or other networks types. [00324] The methods, program codes, and instructions described herein and elsewhere may be implemented on or through mobile devices. The mobile devices may include navigation devices, cell phones, mobile phones, mobile personal digital assistants, laptops, palmtops, netbooks, pagers, electronic books readers, music players and the like. These devices may include, apart from other components, a storage medium such as a flash memory, buffer, RAM, ROM and one or more computing devices. The computing devices associated with mobile devices may be enabled to execute program codes, methods, and instructions stored thereon. Alternatively, the mobile devices may be configured to execute instructions in collaboration with other devices. The mobile devices may communicate with base stations interfaced with servers and configured to execute program codes. The mobile devices may communicate on a peer-to-peer network, mesh network, or other communications network. The program code may be stored on the storage medium associated with the server and executed by a computing device embedded within the server. The base station may include a computing device and a storage medium. The storage device may store program codes and instructions executed by the computing devices associated with the base station.
[00325] The computer software, program codes, and/or instructions may be stored and/or accessed on machine readable media that may include: computer components, devices, and recording media that retain digital data used for computing for some interval of time; semiconductor storage known as random access memory (RAM); mass storage typically for more permanent storage, such as optical discs, forms of magnetic storage like hard disks, tapes, drums, cards and other types ; processor registers, cache memory, volatile memory, non- volatile memory; optical storage such as CD, DVD; removable media such as flash memory (e.g. USB sticks or keys), floppy disks, magnetic tape, paper tape, punch cards, standalone RAM disks, Zip drives, removable mass storage, off-line, and the like; other computer memory such as dynamic memory, static memory, read/write storage, mutable storage, read only, random access, sequential access, location addressable, file addressable, content addressable, network attached storage, storage area network, bar codes, magnetic ink, and the like.
[00326] The methods and systems described herein may transform physical and/or or intangible items from one state to another. The methods and systems described herein may also transform data representing physical and/or intangible items from one state to another.
[00327] The elements described and depicted herein, including in flow charts and block diagrams throughout the figures, imply logical boundaries between the elements. However, according to software or hardware engineering practices, the depicted elements and the functions thereof may be implemented on machines through computer executable media has sender-controlled contact media content item a processor capable of executing program instructions stored thereon as a monolithic software structure, as standalone software modules, or as modules that employ external routines, code, services, and so forth, or any combination of these, and all such implementations may be within the scope of the present disclosure. Examples of such machines may include, but may not be limited to, personal digital assistants, laptops, personal computers, mobile phones, other handheld computing devices, medical equipment, wired or wireless communication devices, transducers, chips, calculators, satellites, tablet PCs, electronic books, gadgets, electronic devices, devices has sender-controlled contact media content item artificial intelligence, computing devices, networking equipment, servers, routers and the like. Furthermore, the elements depicted in the flow chart and block diagrams or any other logical component may be implemented on a machine capable of executing program instructions. Thus, while the foregoing drawings and descriptions set forth functional aspects of the disclosed systems, no particular arrangement of software for implementing these functional aspects should be inferred from these descriptions unless explicitly stated or otherwise clear from the context. Similarly, it will be appreciated that the various steps identified and described above may be varied, and that the order of steps may be adapted to particular applications of the techniques disclosed herein. All such variations and modifications are intended to fall within the scope of this disclosure. As such, the depiction and/or description of an order for various steps should not be understood to require a particular order of execution for those steps, unless required by a particular application, or explicitly stated or otherwise clear from the context.
[00328] The methods and/or processes described above, and steps associated therewith, may be realized in hardware, software or any combination of hardware and software suitable for a particular application. The hardware may include a general- purpose computer and/or dedicated computing device or specific computing device or particular aspect or component of a specific computing device. The processes may be realized in one or more microprocessors, microcontrollers, embedded microcontrollers, programmable digital signal processors or other programmable device, along with internal and/or external memory. The processes may also, or instead, be embodied in an application specific integrated circuit, a programmable gate array, programmable array logic, or any other device or combination of devices that may be configured to process electronic signals. It will further be appreciated that one or more of the processes may be realized as a computer executable code capable of being executed on a machine -readable medium. [00329] The computer executable code may be created using a structured programming language such as C, an object oriented programming language such as C++, or any other high- level or low-level programming language (including assembly languages, hardware description languages, and database programming languages and technologies) that may be stored, compiled or interpreted to run on one of the above devices, as well as heterogeneous combinations of processors, processor architectures, or combinations of different hardware and software, or any other machine capable of executing program instructions.
[00330] Thus, in one aspect, methods described above and combinations thereof may be embodied in computer executable code that, when executing on one or more computing devices, performs the steps thereof. In another aspect, the methods may be embodied in systems that perform the steps thereof, and may be distributed across devices in a number of ways, or all of the functionality may be integrated into a dedicated, standalone device or other hardware. In another aspect, the means for performing the steps associated with the processes described above may include any of the hardware and/or software described above. All such permutations and combinations are intended to fall within the scope of the present disclosure.
[00331] While the disclosure has been disclosed in connection with the preferred embodiments shown and described in detail, various modifications and improvements thereon will become readily apparent to those skilled in the art. Accordingly, the spirit and scope of the present disclosure is not to be limited by the foregoing examples, but is to be understood in the broadest sense allowable by law.
[00332] The use of the terms "a" and "an" and "the" and similar referents in the contest of describing the disclosure (especially in the context of the following claims) is to be construed to cover both the singular and the plural, unless otherwise indicated herein or clearly contradicted by context. The terms "comprising," "has sender-controlled contact media content item," "including," and "containing" are to be construed as open-ended terms (i.e., meaning "including, but not limited to,") unless otherwise noted. Recitation of ranges of values herein are merely intended to serve as a shorthand method of referring individually to each separate value falling within the range, unless otherwise indicated herein, and each separate value is incorporated into the specification as if it were individually recited herein. All methods described herein can be performed in any suitable order unless otherwise indicated herein or otherwise clearly contradicted by context. The use of any and all examples, or exemplary language (e.g., "such as") provided herein, is intended merely to better illuminate the disclosure and does not pose a limitation on the scope of the disclosure unless otherwise claimed. No language in the specification should be construed as indicating any non-claimed element as essential to the practice of the disclosure.
[00333] While the foregoing written description enables one of ordinary skill to make and use what is considered presently to be the best mode thereof, those of ordinary skill will understand and appreciate the existence of variations, combinations, and equivalents of the specific embodiment, method, and examples herein. The disclosure should therefore not be limited by the above described embodiment, method, and examples, but by all embodiments and methods within the scope and spirit of the disclosure,
[00334] All documents referenced herein are hereby incorporated by reference.

Claims

CLAIMS What is claimed is:
1. A method comprising:
receiving data from an IMU device at a first computational module at a first frequency and computing, based at least in part on the received IMU data, a first estimated position of a mobile mapping system;
receiving the first estimated position and visual-inertial data at a second
computational module at a second frequency and computing, based at least in part on the first estimated position and visual-inertial data, a second estimated position of the mobile mapping system; and
receiving the second estimated position and laser scan data at a third computational module at a third frequency and computing, based at least in part on the second estimated position and laser scan data, a third estimated position of the mobile mapping system.
2. The method of claim 1, wherein the each of the first frequency, the second frequency and the third frequency are different from one another.
3. The method of claim 2 where in the frequency is greater than the second frequency and the second frequency is greater than the third frequency.
4. The method of claim 1, wherein the operation of at least one computational module is bypassed.
5. The method of claim 4, wherein the estimated positions computed by each of the remaining non-bypassed modules are combined in a linear fashion.
6. The method of claim 4, wherein the estimated positions computed by each of the remaining non-bypassed modules are combined in a non- linear fashion.
7. The method of claim 4, wherein when the first computational module is bypassed, the second estimated position is computed from the visual inertial data without reference to a first estimated position.
8. The method of claim 4, wherein when the second computational module is bypassed, the third estimated position is computed from the laser scan data and the first estimated position.
9. The method of claim 1, further comprising determining a degraded subspace in a problem state space and computing an estimated position in the degraded subspace.
10. The method of claim 9, wherein the subspace is a well conditioned subspace.
11. The method of claim 1, wherein the estimated position computed by at least one of the second computational module or the third computational module is received as input by a previous computational module,
12. The method of claim 1, wherein the system is adapted to operate at high angular speeds.
13. The method of claim 12, wherein the high angular speeds comprise rotational rates as high as 360 degrees/second.
14. The method of claim 1, wherein the system is adapted to operate at high linear speeds.
15. The method of claim 12, wherein the high linear speeds comprise speeds as high as 100 kilometers/hour.
16. A mobile mapping system comprising:
a first computational module adapted to receive data from an IMU device at a first frequency and compute, based at least in part on the received IMU data, a first estimated position of the mobile mapping system;
a second computational module adapted to receive the first estimated position and visual-inertial data at a second frequency and compute, based at least in part on the first estimated position and visual-inertial data, a second estimated position of the mobile mapping system; and
a third computational module adapted to receive the second estimated position and laser scan data at a third frequency and compute, based at least in part on the second estimated position and laser scan data, a third estimated position of the mobile mapping system.
17. The system of claim 16, wherein the each of the first frequency, the second frequency and the third frequency are different from one another.
18. The system of claim 17 where in the frequency is greater than the second frequency and the second frequency is greater than the third frequency.
19. The system of claim 16, wherein the operation of at least one computational module is bypassed.
20. The system of claim 19, wherein the estimated positions computed by each of the remaining non-bypassed modules are combined in a linear fashion.
21. The system of claim 19, wherein the estimated positions computed by each of the remaining non-bypassed modules are combined in a non- linear fashion.
22. The system of claim 19, wherein when the first computational module is bypassed, the second estimated position is computed from the visual inertial data without reference to a first estimated position.
23. The system of claim 19, wherein when the second computational module is bypassed, the third estimated position is computed from the laser scan data and the first estimated position.
24. The system of claim 16, further comprising determining a degraded subspace in a problem state space and computing an estimated position in the degraded subspace.
25. The system of claim 24, wherein the subspace is a well conditioned subspace.
26. The system of claim 16, wherein the estimated position computed by at least one of the second computational module or the third computational module is received as input by a previous computational module,
27. The system of claim 16, wherein the system is adapted to operate at high angular speeds.
28. The system of claim 27, wherein the high angular speeds comprise rotational rates as high as 360 degrees/second.
29. The system of claim 16, wherein the system is adapted to operate at high linear speeds.
30. The system of claim 27, wherein the high linear speeds comprise speeds as high as 100 kilometers/hour.
PCT/US2018/015403 2016-03-11 2018-01-26 Laser scanner with real-time, online ego-motion estimation WO2018140701A1 (en)

Priority Applications (11)

Application Number Priority Date Filing Date Title
JP2019540661A JP7141403B2 (en) 2017-01-27 2018-01-26 Laser scanner with real-time online self-motion estimation
EP18745179.4A EP3574285A4 (en) 2017-01-27 2018-01-26 Laser scanner with real-time, online ego-motion estimation
EP18824609.4A EP3646058A4 (en) 2017-06-30 2018-06-29 Systems and methods for improvements in scanning and mapping
PCT/US2018/040269 WO2019006289A1 (en) 2017-06-30 2018-06-29 Systems and methods for improvements in scanning and mapping
EP18834521.9A EP3656138A4 (en) 2017-07-17 2018-07-16 Aligning measured signal data with slam localization data and uses thereof
PCT/US2018/042346 WO2019018315A1 (en) 2017-07-17 2018-07-16 Aligning measured signal data with slam localization data and uses thereof
US16/520,503 US20190346271A1 (en) 2016-03-11 2019-07-24 Laser scanner with real-time, online ego-motion estimation
US16/733,787 US11573325B2 (en) 2016-03-11 2020-01-03 Systems and methods for improvements in scanning and mapping
US16/745,775 US10989542B2 (en) 2016-03-11 2020-01-17 Aligning measured signal data with slam localization data and uses thereof
US17/202,602 US11506500B2 (en) 2016-03-11 2021-03-16 Aligning measured signal data with SLAM localization data and uses thereof
US17/964,307 US20230288209A1 (en) 2016-03-11 2022-10-12 Aligning measured signal data with slam localization data and uses thereof

Applications Claiming Priority (6)

Application Number Priority Date Filing Date Title
US201762451294P 2017-01-27 2017-01-27
US62/451,294 2017-01-27
PCT/US2017/021120 WO2017155970A1 (en) 2016-03-11 2017-03-07 Laser scanner with real-time, online ego-motion estimation
USPCT/US2017/021120 2017-03-07
PCT/US2017/055938 WO2018071416A1 (en) 2016-10-11 2017-10-10 Laser scanner with real-time, online ego-motion estimation
USPCT/US2017/055938 2017-10-10

Related Parent Applications (5)

Application Number Title Priority Date Filing Date
PCT/US2017/021120 Continuation WO2017155970A1 (en) 2016-03-11 2017-03-07 Laser scanner with real-time, online ego-motion estimation
PCT/US2017/021120 Continuation-In-Part WO2017155970A1 (en) 2016-03-11 2017-03-07 Laser scanner with real-time, online ego-motion estimation
PCT/US2017/055938 Continuation-In-Part WO2018071416A1 (en) 2016-03-11 2017-10-10 Laser scanner with real-time, online ego-motion estimation
PCT/US2017/055938 Continuation WO2018071416A1 (en) 2016-03-11 2017-10-10 Laser scanner with real-time, online ego-motion estimation
PCT/US2018/015403 Continuation-In-Part WO2018140701A1 (en) 2016-03-11 2018-01-26 Laser scanner with real-time, online ego-motion estimation

Related Child Applications (6)

Application Number Title Priority Date Filing Date
PCT/US2017/021120 Continuation WO2017155970A1 (en) 2016-03-11 2017-03-07 Laser scanner with real-time, online ego-motion estimation
PCT/US2018/015403 Continuation-In-Part WO2018140701A1 (en) 2016-03-11 2018-01-26 Laser scanner with real-time, online ego-motion estimation
PCT/US2018/040269 Continuation-In-Part WO2019006289A1 (en) 2016-03-11 2018-06-29 Systems and methods for improvements in scanning and mapping
PCT/US2018/040269 Continuation WO2019006289A1 (en) 2016-03-11 2018-06-29 Systems and methods for improvements in scanning and mapping
PCT/US2018/042346 Continuation WO2019018315A1 (en) 2016-03-11 2018-07-16 Aligning measured signal data with slam localization data and uses thereof
US16/520,503 Continuation US20190346271A1 (en) 2016-03-11 2019-07-24 Laser scanner with real-time, online ego-motion estimation

Publications (1)

Publication Number Publication Date
WO2018140701A1 true WO2018140701A1 (en) 2018-08-02

Family

ID=62979592

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/US2018/015403 WO2018140701A1 (en) 2016-03-11 2018-01-26 Laser scanner with real-time, online ego-motion estimation

Country Status (3)

Country Link
EP (1) EP3574285A4 (en)
JP (1) JP7141403B2 (en)
WO (1) WO2018140701A1 (en)

Cited By (31)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109945890A (en) * 2018-11-21 2019-06-28 财团法人车辆研究测试中心 More positioning systems switch and merge bearing calibration and its device
CN110132306A (en) * 2019-05-20 2019-08-16 广州小鹏汽车科技有限公司 The correcting method and system of vehicle location error
CN110132302A (en) * 2019-05-20 2019-08-16 中国科学院自动化研究所 Merge binocular vision speedometer localization method, the system of IMU information
CN110243358A (en) * 2019-04-29 2019-09-17 武汉理工大学 The unmanned vehicle indoor and outdoor localization method and system of multi-source fusion
CN110243375A (en) * 2019-06-26 2019-09-17 汕头大学 Method that is a kind of while constructing two-dimensional map and three-dimensional map
CN110298103A (en) * 2019-06-25 2019-10-01 中国电建集团成都勘测设计研究院有限公司 The steep Dangerous Rock Body investigation method of height based on unmanned aerial vehicle onboard three-dimensional laser scanner
DE102018214694A1 (en) * 2018-08-30 2020-03-05 Continental Automotive Gmbh Localization device for the visual localization of a vehicle
JP2020042329A (en) * 2018-09-06 2020-03-19 株式会社ユニックス Building material preprocessing system and building material preprocessing method
CN111009036A (en) * 2019-12-10 2020-04-14 北京歌尔泰克科技有限公司 Grid map correction method and device in synchronous positioning and map construction
EP3674661A1 (en) * 2018-12-29 2020-07-01 Ninebot (Changzhou) Tech Co., Ltd. Locating method and device, storage medium, and electronic device
WO2020138596A1 (en) 2018-12-23 2020-07-02 Samsung Electronics Co., Ltd. Method and device for performing a loop closure detection
EP3731187A1 (en) 2019-04-26 2020-10-28 Volkswagen Ag Method and device for determining the geographical position and orientation of a vehicle
EP3646058A4 (en) * 2017-06-30 2020-12-02 Kaarta, Inc. Systems and methods for improvements in scanning and mapping
US20210025998A1 (en) * 2018-04-03 2021-01-28 Kaarta, Inc. Methods and systems for real or near real-time point cloud map data confidence evaluation
US10962370B2 (en) 2016-03-11 2021-03-30 Kaarta, Inc. Laser scanner with real-time, online ego-motion estimation
CN112654545A (en) * 2018-09-07 2021-04-13 图森有限公司 Backward sensing system for vehicle
GB2587794A (en) * 2019-08-27 2021-04-14 Hybird Ltd Inspection apparatus and method
US10989542B2 (en) 2016-03-11 2021-04-27 Kaarta, Inc. Aligning measured signal data with slam localization data and uses thereof
US20210150923A1 (en) * 2019-11-20 2021-05-20 Thales Electronic device and method for estimating the position(s) of air traffic element(s), related display system and computer program
WO2021109166A1 (en) * 2019-12-06 2021-06-10 苏州艾吉威机器人有限公司 Three-dimensional laser positioning method and system
US20210404814A1 (en) * 2020-06-30 2021-12-30 Lyft, Inc. Map Generation Using Two Sources of Sensor Data
US20220042824A1 (en) * 2019-02-28 2022-02-10 Brain Corporation Systems, and methods for merging disjointed map and route data with respect to a single origin for autonomous robots
US11398075B2 (en) 2018-02-23 2022-07-26 Kaarta, Inc. Methods and systems for processing and colorizing point clouds and meshes
US11474247B2 (en) 2018-11-13 2022-10-18 Beijing Didi Infinity Technology And Development Co., Ltd. Methods and systems for color point cloud generation
DE102021117311A1 (en) 2021-07-05 2023-01-05 Spleenlab GmbH Control and navigation device for an autonomously moving system and autonomously moving system
US11567201B2 (en) 2016-03-11 2023-01-31 Kaarta, Inc. Laser scanner with real-time, online ego-motion estimation
US11573325B2 (en) 2016-03-11 2023-02-07 Kaarta, Inc. Systems and methods for improvements in scanning and mapping
US11815601B2 (en) 2017-11-17 2023-11-14 Carnegie Mellon University Methods and systems for geo-referencing mapping systems
US11830136B2 (en) 2018-07-05 2023-11-28 Carnegie Mellon University Methods and systems for auto-leveling of point clouds and 3D models
WO2024013108A1 (en) * 2022-07-11 2024-01-18 Cambridge Enterprise Limited 3d scene capture system
EP4083739A4 (en) * 2020-01-20 2024-01-24 Shenzhen Pudu Tech Co Ltd Multi-sensor fusion slam system, multi-sensor fusion method, robot, and medium

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP4246489A1 (en) * 2020-11-13 2023-09-20 Pioneer Corporation Information processing apparatus, control method, program, and storage medium

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20080033645A1 (en) * 2006-08-03 2008-02-07 Jesse Sol Levinson Pobabilistic methods for mapping and localization in arbitrary outdoor environments
US20110178708A1 (en) * 2010-01-18 2011-07-21 Qualcomm Incorporated Using object to align and calibrate inertial navigation system
US20140333741A1 (en) * 2013-05-08 2014-11-13 Regents Of The University Of Minnesota Constrained key frame localization and mapping for vision-aided inertial navigation
WO2015105597A2 (en) 2013-11-27 2015-07-16 The Trustees Of The University Of Pennsylvania Multi-sensor fusion for robust autonomous flight in indoor and outdoor environments with a rotorcraft micro-aerial vehicle (mav)
EP3109589A1 (en) 2015-06-23 2016-12-28 Volvo Car Corporation A unit and method for improving positioning accuracy

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP5342556B2 (en) * 2008-07-10 2013-11-13 三菱電機株式会社 Convoy travel support device
GB2489179B (en) * 2010-02-05 2017-08-02 Trimble Navigation Ltd Systems and methods for processing mapping and modeling data
EP3324152B1 (en) * 2015-07-13 2019-04-03 Nissan Motor Co., Ltd. Own-position estimating device and own-position estimating method
CN109313024B (en) * 2016-03-11 2022-06-17 卡尔塔股份有限公司 Laser scanner with real-time online self-motion estimation
JP2019532433A (en) * 2016-10-11 2019-11-07 カールタ インコーポレイテッド Laser scanner with real-time online egomotion estimation

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20080033645A1 (en) * 2006-08-03 2008-02-07 Jesse Sol Levinson Pobabilistic methods for mapping and localization in arbitrary outdoor environments
US20110178708A1 (en) * 2010-01-18 2011-07-21 Qualcomm Incorporated Using object to align and calibrate inertial navigation system
US20140333741A1 (en) * 2013-05-08 2014-11-13 Regents Of The University Of Minnesota Constrained key frame localization and mapping for vision-aided inertial navigation
WO2015105597A2 (en) 2013-11-27 2015-07-16 The Trustees Of The University Of Pennsylvania Multi-sensor fusion for robust autonomous flight in indoor and outdoor environments with a rotorcraft micro-aerial vehicle (mav)
EP3109589A1 (en) 2015-06-23 2016-12-28 Volvo Car Corporation A unit and method for improving positioning accuracy

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
M. KAESSA. RANGANATHANF. DELLAERT: "iSAM: Incremental Smoothing and Mapping", IEEE TRANS. ON ROBOTICS, vol. 24, no. 6, December 2008 (2008-12-01), pages 1365 - 1378
See also references of EP3574285A4

Cited By (46)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11506500B2 (en) 2016-03-11 2022-11-22 Kaarta, Inc. Aligning measured signal data with SLAM localization data and uses thereof
US10989542B2 (en) 2016-03-11 2021-04-27 Kaarta, Inc. Aligning measured signal data with slam localization data and uses thereof
US10962370B2 (en) 2016-03-11 2021-03-30 Kaarta, Inc. Laser scanner with real-time, online ego-motion estimation
US11567201B2 (en) 2016-03-11 2023-01-31 Kaarta, Inc. Laser scanner with real-time, online ego-motion estimation
US11573325B2 (en) 2016-03-11 2023-02-07 Kaarta, Inc. Systems and methods for improvements in scanning and mapping
US11585662B2 (en) 2016-03-11 2023-02-21 Kaarta, Inc. Laser scanner with real-time, online ego-motion estimation
EP3646058A4 (en) * 2017-06-30 2020-12-02 Kaarta, Inc. Systems and methods for improvements in scanning and mapping
US11815601B2 (en) 2017-11-17 2023-11-14 Carnegie Mellon University Methods and systems for geo-referencing mapping systems
US11398075B2 (en) 2018-02-23 2022-07-26 Kaarta, Inc. Methods and systems for processing and colorizing point clouds and meshes
US20210025998A1 (en) * 2018-04-03 2021-01-28 Kaarta, Inc. Methods and systems for real or near real-time point cloud map data confidence evaluation
US11830136B2 (en) 2018-07-05 2023-11-28 Carnegie Mellon University Methods and systems for auto-leveling of point clouds and 3D models
DE102018214694A1 (en) * 2018-08-30 2020-03-05 Continental Automotive Gmbh Localization device for the visual localization of a vehicle
JP2020042329A (en) * 2018-09-06 2020-03-19 株式会社ユニックス Building material preprocessing system and building material preprocessing method
CN112654545B (en) * 2018-09-07 2023-12-05 图森有限公司 Rear sensing system for vehicle
US11704909B2 (en) 2018-09-07 2023-07-18 Tusimple, Inc. Rear-facing perception system for vehicles
CN112654545A (en) * 2018-09-07 2021-04-13 图森有限公司 Backward sensing system for vehicle
US11474247B2 (en) 2018-11-13 2022-10-18 Beijing Didi Infinity Technology And Development Co., Ltd. Methods and systems for color point cloud generation
CN109945890A (en) * 2018-11-21 2019-06-28 财团法人车辆研究测试中心 More positioning systems switch and merge bearing calibration and its device
CN109945890B (en) * 2018-11-21 2022-01-25 财团法人车辆研究测试中心 Multi-positioning system switching and fusion correction method and device
CN112955901A (en) * 2018-12-23 2021-06-11 三星电子株式会社 Method and apparatus for performing loop detection
WO2020138596A1 (en) 2018-12-23 2020-07-02 Samsung Electronics Co., Ltd. Method and device for performing a loop closure detection
EP3834133A4 (en) * 2018-12-23 2021-10-13 Samsung Electronics Co., Ltd. Method and device for performing a loop closure detection
CN111380515A (en) * 2018-12-29 2020-07-07 纳恩博(常州)科技有限公司 Positioning method and device, storage medium and electronic device
EP3674661A1 (en) * 2018-12-29 2020-07-01 Ninebot (Changzhou) Tech Co., Ltd. Locating method and device, storage medium, and electronic device
CN111380515B (en) * 2018-12-29 2023-08-18 纳恩博(常州)科技有限公司 Positioning method and device, storage medium and electronic device
US11067694B2 (en) 2018-12-29 2021-07-20 Ninebot (Changzhou) Tech Co., Ltd. Locating method and device, storage medium, and electronic device
US20220042824A1 (en) * 2019-02-28 2022-02-10 Brain Corporation Systems, and methods for merging disjointed map and route data with respect to a single origin for autonomous robots
EP3731187A1 (en) 2019-04-26 2020-10-28 Volkswagen Ag Method and device for determining the geographical position and orientation of a vehicle
US11573091B2 (en) 2019-04-26 2023-02-07 Volkswagen Aktiengesellschaft Method and device for determining the geographic position and orientation of a vehicle
CN110243358A (en) * 2019-04-29 2019-09-17 武汉理工大学 The unmanned vehicle indoor and outdoor localization method and system of multi-source fusion
CN110243358B (en) * 2019-04-29 2023-01-03 武汉理工大学 Multi-source fusion unmanned vehicle indoor and outdoor positioning method and system
CN110132306B (en) * 2019-05-20 2021-02-19 广州小鹏汽车科技有限公司 Method and system for correcting vehicle positioning error
CN110132302A (en) * 2019-05-20 2019-08-16 中国科学院自动化研究所 Merge binocular vision speedometer localization method, the system of IMU information
CN110132306A (en) * 2019-05-20 2019-08-16 广州小鹏汽车科技有限公司 The correcting method and system of vehicle location error
CN110298103A (en) * 2019-06-25 2019-10-01 中国电建集团成都勘测设计研究院有限公司 The steep Dangerous Rock Body investigation method of height based on unmanned aerial vehicle onboard three-dimensional laser scanner
CN110243375A (en) * 2019-06-26 2019-09-17 汕头大学 Method that is a kind of while constructing two-dimensional map and three-dimensional map
GB2587794A (en) * 2019-08-27 2021-04-14 Hybird Ltd Inspection apparatus and method
US20210150923A1 (en) * 2019-11-20 2021-05-20 Thales Electronic device and method for estimating the position(s) of air traffic element(s), related display system and computer program
WO2021109166A1 (en) * 2019-12-06 2021-06-10 苏州艾吉威机器人有限公司 Three-dimensional laser positioning method and system
CN111009036A (en) * 2019-12-10 2020-04-14 北京歌尔泰克科技有限公司 Grid map correction method and device in synchronous positioning and map construction
CN111009036B (en) * 2019-12-10 2023-11-21 北京歌尔泰克科技有限公司 Grid map correction method and device in synchronous positioning and map construction
EP4083739A4 (en) * 2020-01-20 2024-01-24 Shenzhen Pudu Tech Co Ltd Multi-sensor fusion slam system, multi-sensor fusion method, robot, and medium
US20210404814A1 (en) * 2020-06-30 2021-12-30 Lyft, Inc. Map Generation Using Two Sources of Sensor Data
EP4116790A1 (en) 2021-07-05 2023-01-11 Spleenlab GmbH Autonomously moving system and control and navigation device for an autonomously moving system
DE102021117311A1 (en) 2021-07-05 2023-01-05 Spleenlab GmbH Control and navigation device for an autonomously moving system and autonomously moving system
WO2024013108A1 (en) * 2022-07-11 2024-01-18 Cambridge Enterprise Limited 3d scene capture system

Also Published As

Publication number Publication date
JP7141403B2 (en) 2022-09-22
EP3574285A1 (en) 2019-12-04
JP2020507072A (en) 2020-03-05
EP3574285A4 (en) 2020-12-02

Similar Documents

Publication Publication Date Title
US11506500B2 (en) Aligning measured signal data with SLAM localization data and uses thereof
US11567201B2 (en) Laser scanner with real-time, online ego-motion estimation
US20190346271A1 (en) Laser scanner with real-time, online ego-motion estimation
JP7141403B2 (en) Laser scanner with real-time online self-motion estimation
WO2018071416A1 (en) Laser scanner with real-time, online ego-motion estimation
WO2019018315A1 (en) Aligning measured signal data with slam localization data and uses thereof
US10962370B2 (en) Laser scanner with real-time, online ego-motion estimation
US10096129B2 (en) Three-dimensional mapping of an environment
Weiss et al. Intuitive 3D maps for MAV terrain exploration and obstacle avoidance
Cui et al. Drones for cooperative search and rescue in post-disaster situation
JP2023072064A (en) Performing 3d reconstruction via unmanned aerial vehicle
Sanfourche et al. Perception for UAV: Vision-Based Navigation and Environment Modeling.
US20210311504A1 (en) Near real-time reconstruction using drones
Klingbeil et al. Towards autonomous navigation of an UAV-based mobile mapping system
Kalisperakis et al. A modular mobile mapping platform for complex indoor and outdoor environments
Steenbeek CNN based dense monocular visual SLAM for indoor mapping and autonomous exploration
George Analysis of Visual-Inertial Odometry Algorithms for Outdoor Drone Applications
Sanfourche et al. 3DSCAN: Online ego-localization and environment mapping for micro aerial vehicles
Wendel Scalable visual navigation for micro aerial vehicles using geometric prior knowledge
Ji Robust visual SLAM for autonomous vehicles in challenging environments
Radford Real-time roadway mapping and ground robotic path planning via unmanned aircraft
Nejad Design and implementation of a localization system for surveying robot based on visual odometry
Merino et al. Single and multi-UAV relative position estimation based on natural landmarks
Wei Enhancing the Accuracy and Robustness of LiDAR Based Simultaneous Localisation and Mapping
Mostofi 3D Indoor Mobile Mapping using Multi-Sensor Autonomous Robot

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

Country of ref document: EP

Kind code of ref document: A1

ENP Entry into the national phase

Ref document number: 2019540661

Country of ref document: JP

Kind code of ref document: A

NENP Non-entry into the national phase

Ref country code: DE

ENP Entry into the national phase

Ref document number: 2018745179

Country of ref document: EP

Effective date: 20190827