JP2020507072A - Laser scanner with real-time online self-motion estimation - Google Patents

Abstract

The method includes receiving data from an IMU device at a first calculation module at a first frequency, and calculating a first estimated position of the mobile mapping system based at least in part on the received IMU data; Estimated position and visual-inertial data are received at a second calculation module at a second frequency, and a second estimated position of the mobile mapping system is calculated based at least in part on the first estimated position and the visual-inertial data. Receiving the second estimated position and the laser scan data at a third frequency at a third calculation module, and determining a third position of the mobile mapping system based at least in part on the second estimated position and the laser scan data. Calculating the estimated position of [Selection diagram] FIG.

Description

  The present invention relates to a mobile mapping system.

(Priority claim)
This application is PCT Application No. PCT / US2017 / 055938, filed October 10, 2017, entitled "LASER SCANNER WITH REAL-TIME, ONLINE EGO-MOTION ESTIMATION". (Attorney Docket No. KRTA-0008-WO) and is a continuation-in-part application.

  PCT Application No. PCT / US2017 / 055938 is a PCT application entitled "Laser Scanner WITH REAL-TIME, ONLINE EGO-MOTION ESTIMATION" filed March 7, 2017 filed on March 7, 2017. No. PCT / US2017 / 021120 (Attorney Docket No. KRTA-0005-WO) and a continuation-in-part application. This application also claims priority to PCT Application No. PCT / US2017 / 021120. PCT Application No. PCT / US2017 / 055938 is a U.S. patent filed October 11, 2016, entitled "LASER SCANNER WITH REAL-TIME, ONLINE EGO-MOTION ESTIMATION". It further claims priority to provisional application No. 62 / 406,910 (Attorney Docket No. KRTA-0002-P02).

  PCT Application No. PCT / US2017 / 021120 is a U.S. patent filed March 11, 2016, entitled "LASER SCANNER WITH REAL-TIME, ONLINE EGO-MOTION Estimation". The benefit of provisional application No. 62 / 307,061 (Attorney Docket No. KRTA-0001-P01) is claimed.

  This application is a provisional U.S. patent application Ser. No. 62 / 451,294 filed Jan. 27, 2017, entitled "LIDAR AND VISION-BASED EGO-MOTION ESTIMATION AND MAPPING." No. (Attorney Docket No. KRTA-0004-P01).

(Cross-reference of related applications)
The disclosures of PCT / US2017 / 055938, PCT / US2017 / 021120, and US Provisional Patent Applications Nos. 62 / 406,910, 62 / 307,061 and 62 / 451,294 are disclosed. , All of which are incorporated herein by reference for all purposes.

  An autonomous mobile device may need information about the terrain on which it operates. Such a device may rely on either a predefined map representing the terrain and any obstacles that may be found there. Alternatively, the device may have the ability to map its terrain while stationary or exercising, including a computer-based mapping system having one or more sensors that provide real-time data. The mobile computer-based mapping system can estimate the change in position over time (odometer) and / or generate a three-dimensional map representation, such as a point cloud in three-dimensional space.

  An exemplary mapping system can include various sensors that provide data from which a map can be constructed. Some mapping systems may use a stereo camera system as one such sensor. These systems benefit from a baseline between two cameras as a basis for determining the scale of motion estimation. Binocular systems are preferred over monocular systems because monocular systems may not be able to determine the scale of an image without receiving data from additional sensors or making assumptions about the motion of the device Because. In recent years, RGB-D cameras have become popular in the research field. Such a camera can provide depth information associated with individual pixels, and can thus help determine the scale. However, some methods, including RGB-D cameras, can only use image areas with coverage of depth information, resulting in large image areas, especially in open spaces where only sparse depth acquisition is possible. May be wasted.

  In another example of a mapping system, an IMU may be coupled to one or more cameras such that scale constraints can be imposed from IMU acceleration. In some examples, the monocular camera may be tightly or loosely coupled to the IMU using a Kalman filter. Other mapping systems may use optimization methods to solve the motion of the mobile system.

  Alternatives to the mapping system can include the use of a laser scanner for motion estimation. However, the scanning speed of the laser can make the use of such data difficult. While the system is moving, unlike a fixed position laser scanner, the laser point is affected by the relative movement of the scanner. Thus, the effect of this movement may be a factor in the arriving laser points reaching the system at different times. As a result, when the scan speed is slow relative to the movement of the mapping system, there may be scan distortion due to external movement of the laser. The effects of this motion can be compensated for by the laser itself, but this compensation may require an independent motion model to provide the required correction. As an example, motion can be modeled as a constant velocity or as a Gaussian process. In some examples, the IMU may provide a motion model. Such a method aligns the space-time patches formed by the laser point cloud to estimate sensor motion and corrects IMU bias with off-line batch optimization.

  A similar problem of motion distortion may be found when using a rolling shutter camera. In particular, image pixels may be received continuously over time, resulting in image distortion caused by extraneous movement of the camera. In some examples, the visual odometry method may use the IMU to compensate for rolling shutter effects given a pixel readout time.

  In some examples, GPS / INS technology can be used to determine the location of a mobile mapping device. However, a precision GPS / INS solution may not be practical when the application is GPS rejected, lightweight, or cost sensitive. It has been recognized that accurate GPS mapping requires line-of-sight communication between a GPS receiver and at least four GPS satellites, although five would be preferred. In some environments, for example, in urban environments that may include elevated roads and other obstacles, it may be difficult to receive undistorted signals from four satellites.

  That is, some of the technical issues associated with fusing data from optical devices with other motion measurement devices to generate a robust map of the terrain surrounding the autonomous mapping device, particularly while the mapping device is in motion. It will be appreciated that the challenge exists. Disclosed below is a method and system for a mapping device that can obtain optical mapping information and generate a robust map with reduced distortion.

  According to an exemplary and non-limiting embodiment, a method includes receiving data from an IMU device at a first frequency at a first frequency, and first estimating a mobile mapping system based at least in part on the received IMU data. Calculating a position and receiving the first estimated position and visual-inertial data at a second frequency at a second computation module and based at least in part on the first estimated position and visual-inertial data; Calculating a second estimated position of the mapping system; receiving the second estimated position and the laser scan data at a third frequency at a third calculation module and at least partially converting the second estimated position and the laser scan data; Calculating a third estimated position of the mobile mapping system based on the location.

  According to another exemplary and non-limiting embodiment, a mobile mapping system receives data at a first frequency from an IMU device and determines a first estimated location of the mobile mapping system based at least in part on received IMU data. A first calculation module adapted to calculate, and receiving, at a second frequency, the first estimated position and the visual-inertial data and based on the mobile based at least in part on the first estimated position and the visual-inertial data. A second calculation module adapted to calculate a second estimated position of the mapping system; receiving the second estimated position and the laser scan data at a third frequency into a second estimated position and the laser scan data. A third calculation module adapted to calculate a third estimated position of the mobile mapping system based at least in part on: .

FIG. 2 is a block diagram of an embodiment of a mapping system. FIG. 2 illustrates an embodiment of a block diagram of three computation modules of the mapping system of FIG. 1 and their respective feedback features. FIG. 4 is a diagram illustrating an embodiment of a Kalman filter model for refining position information into a map. FIG. 4 is a diagram illustrating an embodiment of a factor graph optimization model for refining position information into a map. FIG. 3 illustrates an embodiment of a visual-inertial odometry subsystem. FIG. 2 illustrates an embodiment of a scan matching subsystem. FIG. 4 illustrates an embodiment of a global map with coarse detail resolution. FIG. 3 illustrates an embodiment of a sub-map with subtle detail resolution. FIG. 4 illustrates an embodiment of multi-thread scan matching. FIG. 4 illustrates an embodiment of single thread scan matching. FIG. 7 illustrates an embodiment of a block diagram of three calculation modules in which feedback data from the visual-inertial odometry unit is suppressed due to data degradation. FIG. 4 illustrates an embodiment of a block diagram of three calculation modules where feedback data from a scan matching unit is suppressed due to data degradation. FIG. 4 illustrates an embodiment of a block diagram of three computation modules where feedback data from the visual-inertial odometry unit and the scan matching unit is partially suppressed due to data degradation. FIG. 4 illustrates an embodiment of an estimated trajectory of a mobile mapping device. FIG. 4 illustrates an interactive information flow according to an exemplary and non-limiting embodiment. FIG. 2 illustrates a dynamically reconfigurable system according to an exemplary and non-limiting embodiment. FIG. 2 illustrates a dynamically reconfigurable system according to an exemplary and non-limiting embodiment. FIG. 4 illustrates priority feedback for IMU bias correction according to an exemplary and non-limiting embodiment. FIG. 4 illustrates a two-layer voxel representation of a map according to an exemplary and non-limiting embodiment. FIG. 4 illustrates a two-layer voxel representation of a map according to an exemplary and non-limiting embodiment. FIG. 4 illustrates a multi-threaded process of scan matching according to an exemplary and non-limiting embodiment. FIG. 4 illustrates a multi-threaded process of scan matching according to an exemplary and non-limiting embodiment. FIG. 2 illustrates an exemplary, non-limiting embodiment of a SLAM system. FIG. 2 illustrates an exemplary, non-limiting embodiment of a SLAM system. FIG. 3 illustrates an exemplary, non-limiting embodiment of a SLAM enclosure. FIG. 4 illustrates an exemplary, non-limiting embodiment of a point cloud indicating a level of trust. FIG. 4 illustrates an exemplary, non-limiting embodiment of a point cloud indicating a level of trust. FIG. 4 illustrates an exemplary, non-limiting embodiment of a point cloud indicating a level of trust. FIG. 4 illustrates an exemplary, non-limiting embodiment of different confidence level metrics. FIG. 2 illustrates an exemplary, non-limiting embodiment of a SLAM system. FIG. 3 illustrates an exemplary, non-limiting embodiment of a timing signal for a SLAM system. FIG. 3 illustrates an exemplary, non-limiting embodiment of a timing signal for a SLAM system. FIG. 4 illustrates an exemplary, non-limiting embodiment of SLAM system signal synchronization. FIG. 3 illustrates an exemplary, non-limiting embodiment of air-ground coordinated mapping. FIG. 4 illustrates an exemplary, non-limiting embodiment of a sensor pack. FIG. 3 illustrates an exemplary, non-limiting embodiment of a block diagram of a laser-vision-inertial odometry and mapping software system. FIG. 3 illustrates an exemplary, non-limiting embodiment of a comparison of scans involved in odometry estimation and localization. FIG. 3 illustrates an exemplary, non-limiting embodiment of a comparison of scan matching accuracy in localization. FIG. 3 illustrates an exemplary, non-limiting embodiment of a horizontal bearing sensor test. FIG. 3 illustrates an exemplary, non-limiting embodiment of a vertical bearing sensor test. FIG. 4 illustrates an exemplary, non-limiting embodiment of an accuracy comparison between a horizontal orientation sensor test and a downward tilt sensor test. FIG. 4 illustrates an exemplary, non-limiting embodiment of an aircraft having a sensor pack. FIG. 4 illustrates an exemplary, non-limiting embodiment of a sensor trajectory. FIG. 4 illustrates an exemplary, non-limiting embodiment of an autonomous flight result. FIG. 4 illustrates an exemplary, non-limiting embodiment of an autonomous flight result over long term operation.

  In one general aspect, the present invention relates to a mobile computer-based mapping system that estimates position changes over time (odometer) and / or generates a three-dimensional map representation of a three-dimensional space, such as a point cloud. The mapping system can include a plurality of sensors including, but not limited to, an inertial measurement unit (IMU), a camera, and / or a 3D laser scanner. The mapping system is also in communication with the plurality of sensors to estimate changes in the position of the system over time and / or to generate a map representation of the surrounding environment and to process outputs from these sensors. A computer system having at least one processor configured may be included. The mapping system can enable high-frequency low-latency online real-time self-motion estimation with dense and accurate 3D map registration. Embodiments of the present disclosure may include a simultaneous localization and mapping (SLAM) system. SLAM systems can include a multi-dimensional (eg, 3D) laser scanning and ranging system that enables simultaneous localization and mapping in real time, independent of GPS. SLAM systems can generate and manage data about very accurate point clouds resulting from the reflection of laser scans from objects in the environment. Movement of any of the points in the point cloud will cause the points in the point cloud to be reference points to the location so that the SLAM system can maintain an accurate understanding of the location and orientation of the system as it moves through the environment. And is tracked accurately over time.

  In one embodiment, the position and motion resolution of the mobile mapping system can be refined sequentially in a series of coarse to fine updates. In a non-limiting example, a separate computation module can be used to update the position and motion of the mobile mapping system from a coarse resolution with a fast update rate to a fine resolution with a slower update rate. For example, the IMU device may provide data to predict the movement or position of the mapping system at a high update rate to the first calculation module. The visual-inertial odometry system can provide data to the second computation module to improve the motion or position resolution of the mapping system at a low update rate. In addition, the laser scanner can provide data to the third computational scan matching module for further refining the motion estimate at a lower update rate and aligning the map. In one non-limiting example, data from a computation module configured to process position and / or motion fine resolution data comprises a computation module configured to process position and / or motion coarse resolution data. You can give feedback. In another non-limiting example, the computation module incorporates fault tolerance to address sensor degradation issues by automatically bypassing the computation module associated with bad sensor sourcing, bad data, incomplete data, or non-existent data. be able to. Thus, the mapping system can operate in an even darker, textured and unstructured environment in the presence of highly dynamic motion.

  In contrast to existing map generation techniques, which are mostly off-line batch systems, the mapping systems disclosed herein operate in real time and can generate maps while on the move. This feature offers two practical advantages. First, the user is not limited to a scanner that is fixed on a tripod or other non-stationary mount. Instead, the mapping system disclosed herein can associate with a mobile device, thereby extending the range of environments that can be mapped in real time. Second, real-time features can provide feedback to the user regarding the current map area while data is being collected. Online generation maps may also assist robots or other devices for autonomous navigation and obstacle avoidance. In some non-limiting embodiments, such navigation features can be incorporated into the mapping system itself. In an alternative, non-limiting embodiment, the map data can be provided to additional robots with navigation capabilities that may require an external source map.

  There are several potential uses for sensors, such as 3D modeling, scene mapping, and environment inference. The mapping system can provide the point cloud map to other algorithms that take the point cloud as input for further processing. Further, the mapping system can function both indoors and outdoors. Such an embodiment does not require external lighting and can operate in the dark. Embodiments with a camera may cope with sudden movements, although external lighting may be required, and may color the laser point cloud with images from the camera. The SLAM system can build and maintain a point cloud in real time as the user moves through the environment, eg, walks, cycles, drives, flies, and combinations thereof. The map is built in real time as the mapper progresses through the environment. SLAM systems can track thousands of features as points. As the mapper moves, these points are tracked to allow for motion estimation. Thus, the SLAM system operates in real time without relying on external localization techniques such as GPS. In embodiments, multiple (most often a large number) environmental features, such as objects, are used as points for triangulation, and the system determines the position and orientation as it moves through the environment. Numerous location and orientation calculations are performed and updated to maintain an accurate current estimate of. In embodiments, the fixed features (walls, doors, windows, furniture, fixtures, etc.) are used using the relative motion of the features inside the environment so that the fixed features can be used for position and orientation calculations. It can be distinguished from mobile features (people, vehicles, and other mobile items). Underwater SLAM systems can use blue-green lasers to reduce attenuation.

  The mapping system design follows the consideration that the drift in the self-motion estimation has a lower frequency than the frequency of the module itself. Therefore, three calculation modules are arranged in descending order of frequency. High frequency modules are specialized in dealing with extreme movements, and low frequency modules offset drift with respect to preceding modules. In addition, this sequential processing favors the calculation that the module at the front is less responsible for the calculations, runs at a higher frequency, and gives the modules at the rear more time for full processing. Thus, the mapping system can obtain a high level of accuracy while operating in real time online.

  Further, the system can be configured to address sensor degradation. If the camera does not work (for example, due to a dark, dramatic lighting change, or an environment without bumps) or the laser does not work (for example, due to an unstructured environment), the corresponding module Can be bypassed and can be alternated to ensure that the rest of the system functions. In particular, in some exemplary embodiments, the proposed pipeline automatically determines the degraded subspace in the problem state space and partially solves the problem in a well-conditioned subspace. As a result, the final solution is formed by the combination of the "healthy" parts from each module. As a result, the resulting combination of modules used to generate the output is not simply a linear or non-linear combination of the module outputs. In some exemplary embodiments, the output reflects the bypass of one or more entire modules in combination with a linear or non-linear combination of the remaining functional modules. Results of testing the system through a number of experiments have shown that the system can produce high accuracy over several kilometers of navigation and robustness against environmental degradation and extreme movement.

The modular mapping system disclosed below is configured to process data from distance, vision, and inertial sensors for motion estimation and mapping by using a multi-layer optimization structure. The modular mapping system
A function to dynamically reconfigure the calculation module,
• Bypassing failure modes in the computation module completely or partially and combining data from the remaining modules in a manner that addresses degradation of the sensor and / or sensor data, thereby causing data degradation and mobile mapping induced by the environment. The ability to cope with extreme movements of the system, and the ability to cooperatively integrate computing modules to provide real-time performance,
High accuracy, robustness, and low drift can be obtained by incorporating features that can include:

  Disclosed herein is a mapping system for online self-motion estimation using a 3D laser, camera, and IMU. In addition, the estimated motion aligns the laser points to build a map of the mobile environment. In many real-world applications, self-motion and mapping must be performed in real time. In autonomous navigation systems, maps can be very important for motion planning and obstacle avoidance, while motion estimation is important for vehicle control and steering.

FIG. 1 depicts a simple block diagram of a mapping system 100 according to one embodiment of the present invention. Although specific components are disclosed below, such components are provided by way of example only and are not limiting with respect to other equivalent or similar components. Exemplary systems include an IMU system 102 such as an Xsens® MTi-30 IMU, a camera system 104 such as an IDS® UI-1220SE monochrome camera, and Velodyne PUCK® VLP-16. A laser scanner 106 such as a laser scanner. The IMU 102 provides inertial motion data and / or inertial data derived from one or more of an xyz accelerometer, a roll-pitch-yaw gyroscope, and a magnetometer at a first frequency. Can be. In some non-limiting examples, the first frequency can be about 200 Hz. The camera system 104 may have a resolution of 752 × 480 pixels, a horizontal field of view (FOV) of 76 °, and a frame capture rate at a second frequency. In some non-limiting examples, the frame capture rate can operate at a second frequency of about 50 Hz. The laser scanner 106 has a horizontal FOV of 360 ° and a vertical FOV of 30 °, and can receive 0.3 million points per second at a third frequency representing the laser rotation speed. In some non-limiting examples, the third frequency can be about 5 Hz. As depicted in FIG. 1, the laser scanner 106 can be connected to a motor 108 that incorporates an encoder 109 to measure the motor rotation angle. In one non-limiting example, the laser motor encoder 109 may operate at about 0.25 ⁰ resolution.

  IMU 102, camera 104, laser scanner 106, and laser scanner motor encoder 109 may be in data communication with computer system 110, which may include one or more processors 134 associated with it. It can be any computing device having memory 103A and having sufficient processing power and memory to perform the desired odometry and / or mapping. For example, a laptop computer having a 2.6 GHz i7 quad-core processor (two threads on each core, for a total of eight threads) and an embedded GPU memory can be used. In addition, the computer system may include 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 flash ROM. Can be provided. Although specific computing modules (IMU module 122, visual-inertial odometry module 126, and laser scanning module 132) have been disclosed above, such modules are merely exemplary modules having the functions described above, and are not limiting. You have to recognize that it is not a target. Similarly, the types of computing device 110 disclosed above are merely examples of, and are in no way limiting, the types of computing devices that can be used with the sensors described above for the purposes disclosed herein.

  As illustrated in FIG. 1, the mapping system 100 incorporates a computation model that includes individual computation modules that sequentially restore motion in a coarse-to-fine manner (see also FIG. 2). Starting with motion prediction from the IMU 102 (IMU prediction module 122), a visual-inertial tight coupling method (visual-inertial odometry module 126) estimates motion and locally aligns laser points. Subsequently, the scan matching method (scan matching refinement module 132) further refines the estimated motion. Further, the scan matching refinement module 132 aligns the point cloud data 165 to build a map (voxel map 134). The map may also be used by the mapping system as part of an optional navigation system 136. It can be appreciated that the navigation system 136 can be included as a computing module within the on-board computer system, as primary memory, or can include a completely separate system.

  It can be appreciated that each computing module can process data from one of each of the sensor systems. Accordingly, the IMU prediction module 122 generates a coarse map from the data derived from the IMU system 102, and the visual-inertial odometry module 126 processes the further refined data from the camera system 104 and performs scan matching refinement. The conversion module 132 processes the finest resolution data from the laser scanner 106 and the motor encoder 109. In addition, each of the finer resolution modules further processes the data provided by the coarser module. The visual-inertial odometry module 126 refines the mapping data calculated by and received from the IMU prediction module 122. Similarly, scan matching refinement module 132 further processes the data provided by visual-inertial odometry module 126. As disclosed above, each of the sensor systems acquires data at different rates. In one non-limiting example, IMU 102 can update data acquisition at a rate of about 200 Hz, camera 104 can update data acquisition at a rate of about 50 Hz, and laser scanner 106 can update data acquisition at a rate of about 5 Hz. Can update the data acquisition. These rates are non-limiting and can reflect, for example, the data acquisition rate of each sensor. It can be seen that coarser data can be obtained at a faster rate than finer grain data, and can also be processed at a faster rate than finer grain data. Although specific frequency values have been disclosed above for data acquisition and processing by various computing modules, neither absolute frequencies nor their relative frequencies are limiting.

  The mapping data and / or navigation data may be considered to include coarse level data and fine level data. Therefore, in the primary memory (dynamic memory 120), coarse position data in the voxel map 134 that can be accessed by any of the calculation modules 122, 126, 132 can be stored. . A file of detailed map data as point cloud data 165 that the scan matching refinement module 132 can generate is stored in a secondary memory 160 such as a hard disk, flash drive, or other more permanent memory through the processor 150. Can be stored.

  Not only does the calculation module for finer grained calculations use coarser data, but also both the visual-inertial odometry module 126 and the scan matching refinement module 132 (fine grained location information and mapping) The more refined mapping data can be fed back to the IMU prediction module 122 through respective feedback paths 128 and 138 as a basis for updating the IMU location prediction. In this manner, coarse position data and mapping data can be refined in order in resolution, and the refined resolution data serves as a feedback reference for coarse resolution calculations.

  FIG. 2 is a block diagram of the three calculation modules and their respective data paths. The IMU prediction module 122 may receive IMU location data 223 from the IMU (102 in FIG. 1). The visual-inertial odometry module 126 receives model data from the IMU prediction module 122, and additionally receives visual data from one or more individual tracking visual features 227a, 227b from the camera (104 in FIG. 1). be able to. The laser scanner (106 in FIG. 1) generates data relating to the laser decision landmarks 233a, 233b that can be provided to the scan matching refinement module 132 in addition to the position data provided by the visual-inertial odometry module 126. Can be done. The position estimation model from the visual-inertial odometry module 126 can be fed back 128 to refine the position model calculated by the IMU prediction module 122. Similarly, the refined map data from the scan matching refinement module 132 can be fed back 138 to make additional corrections to the position model calculated by the IMU prediction module 122.

  As disclosed above and depicted in FIG. 2, the modular mapping system can sequentially restore and refine motion related data in a coarse to fine manner. In addition to the above, the data processing of each module may be determined by the speed of data acquisition and processing of each of the devices that output data to the module. Starting from motion prediction from the IMU, the vision-inertial tight coupling method can estimate and locally align the laser points. Subsequently, the scan matching method further refines the estimated motion. The scan matching refinement module can also align the point cloud to build a map. As a result, the mapping system is time-optimized to process each refinement step as data becomes available.

  FIG. 3 illustrates a standard Kalman filter model based on data derived from the same type of sensor depicted in FIG. As illustrated in FIG. 3, the Kalman filter model updates the position data and / or mapping data upon receipt of any data from any of the sensors, regardless of the resolution of the data. In this case, for example, the position information can be updated using the visual-inertial odometry data each time such data becomes available regardless of the state of the position information estimation based on the IMU data. Therefore, the Kalman filter model does not make use of the relative resolution of each type of measurement. FIG. 3 depicts a block diagram of a standard Kalman filter based method for optimizing position data. The Kalman filter sequentially updates the position models 322a to 322n as data is provided. Thus, starting from the initial position prediction model 322a, the Kalman filter can predict 324a a subsequent position model 322b, which can be refined based on the received IMU mechanized data 323. The position prediction model can be updated in response to the IMU mechanized data 323 in a prediction step 324a, 322b, followed by an update step in which individual visual features or laser landmarks are seeded.

  FIG. 4 depicts a location optimization based on the factor graph method. In this way, the attitude of the mobile mapping system at the first time 410 can be updated to the attitude at the second time 420 after receiving the data. The factor graph optimization model combines constraints from all sensors during each refinement calculation. In this case, the IMU data 323, feature data 327a, 327b, and similar from the camera, as well as laser landmark data 333a, 333b, etc., are all used for each update stage. It will be appreciated that such a method increases the computational complexity for each location refinement step due to the large amount of data required. Further, since the sensors may provide data at independent rates, which may vary by orders of magnitude, the entire refinement step is time-bound to the data acquisition time for the slowest sensor. As a result, such models may not be suitable for fast real-time mapping. The modularized system depicted in FIGS. 1 and 2 sequentially restores motion in a coarse-to-fine manner. In this way, the degree of motor refinement is determined by the availability of each type of data.

Assumptions, coordinates, and problem assumptions and coordinate systems

As depicted above in FIG. 1, the sensor system of the mobile mapping system can include a laser 106, a camera 104, and an IMU 102. The camera can be modeled as a pinhole camera model whose internal parameters are understood. External parameters between all three sensors can be calibrated. The relative attitude between the camera and the laser and the relative attitude between the laser and the IMU can be determined according to methods known in the art. A single coordinate system can be used for the camera and laser. In one non-limiting example, a camera coordinate system can be used, and all laser points can be projected into the camera coordinate system in pre-processing. In one non-limiting example, the IMU coordinate system can be parallel to the camera coordinate system, where the IMU measurements can be rotationally corrected when acquired. The coordinate system can be defined as follows.
The camera coordinate system {C} can have the origin at the optical center of the camera. In this coordinate system, the x-axis points to the left, the y-axis points upward, and the z-axis points forward and Matches,
The IMU coordinate system {I} can have the origin at the IMU measurement center, in which the x-axis, y-axis, and z-axis are parallel to {C} and oriented in the same direction; In the starting posture, the world coordinate system {W} may be a coordinate system that matches {C}.

  According to some exemplary embodiments, landmark locations are not always optimized. As a result, six unknowns remain in the state space, and thus the computational strength is kept low. The disclosed method of the present invention includes laser distance measurement to provide accurate depth information for features to ensure motion estimation accuracy, while collectively further optimizing feature depth. Only certain portions of these measurements need to be optimized, as in certain circumstances further optimizations may result in reduced return values.

  According to exemplary and non-limiting embodiments, calibration of the described system can be based, at least in part, on the mechanical geometry of the system. In particular, the rider can be calibrated relative to the motor shaft using mechanical measurements from the CAD model of the system with respect to the geometric relationship between the rider and the motor shaft. Such calibrations obtained in connection with a CAD model have been shown to provide high accuracy and drift without having to perform additional calibrations.

MAP Estimation Problem The state estimation problem can be formulated as a maximum posterior probability (MAP) estimation problem. Define｝ = {x _i } ( _iセ ッ ト 1; 2;..., M}) as a system state set, and U = {u _i } (i∈ ｛1; 2;. , And Z = {z _k } ( _k {1; 2;..., N}) can be defined as a landmark measurement value set. Given the proposed system, Z can consist of both visual features and laser landmarks. The joint probability of the system is defined as:
Equation (1)

Before P (x ₀₎ in the formula is a preceding state of the first system _{state, P (x i | x i} -1), u i) represents a motion _{model, P (z k | x ik} ) is Represents a landmark measurement model. For each problem formulated as equation (1), there is a corresponding Bayesian network representation of the problem. MAP estimation is to maximize Equation 1. Under the assumption of zero mean Gaussian noise, this problem is equivalent to the least squares problem.
Equation (2)

R _xi and r _zk in the above _equation are the residual errors associated with the mathematical model and the landmark measurement model, respectively.

The standard approach to solving Equation 2 is to combine all sensor data, eg, visual features, laser landmarks, and IMU measurements, into a large factor graph optimization problem. Conversely, the proposed data processing pipeline formulates a number of small optimization problems and solves these problems in a coarse to fine manner. The optimization problem can be paraphrased as follows:
Problem: Given data from the laser, camera, and IMU, formulate and solve the problem as equation (2) to determine the attitude of {C} relative to {W}, and then use the estimated attitude to solve the laser. Align the points and build a map of the mobile environment with {W}.

式（４）

式中の

は、｛Ｗ｝から｛Ｃ｝への回転行列であり、

は、｛Ｃ｝と｛Ｉ｝との間の平行移動ベクトルである。 IMU Prediction Subsystem IMU Mechanization This subsection describes the IMU prediction subsystem. Since the system considers {C} as the basic sensor coordinate system, the IMU can be characterized for {C}. As disclosed above in the subsection entitled "Assumptions and Coordinate Systems", {I} and {C} are parallel coordinate systems. ω (t) and a (t) are two 3 × 1 vectors indicating the angular velocity and acceleration of {C} at time t, respectively. Expressed the corresponding bias b _omega and (t) and _{b a (t), n ω} (t) and n _a (t) can be a corresponding noise. The terms of vector, bias, and noise are defined by {C}. In addition, g can be described as a constant gravity vector at {W}. The IMU measurement term is:
Equation (3)

Equation (4)

In the formula

Is the rotation matrix from {W} to {C},

Is the translation vector between {C} and {I}.

は、回転中心（｛Ｃ｝の原点）が｛Ｉ｝の原点と異なることに起因する遠心力を表すことに注意されたい。視覚−慣性ナビゲーション方法の一部の例は、この遠心力項を排除するために運動を｛Ｉ｝においてモデル化する。深度情報が既知の視覚特徴部と未知の視覚特徴部との両方を用いる本明細書に開示する計算方法では、深度が未知の特徴部を｛Ｃ｝から｛Ｉ｝へと変換する段階は容易ではない（下記を参照されたい）。その結果、本明細書に開示するシステムは、代わりに運動の全てを｛Ｃ｝においてモデル化する。実際には、この項の効果を最大限に低減するためにカメラとＩＭＵとが互いの近くに装着される。 Terms:

Note that represents the centrifugal force due to the center of rotation (the origin of {C}) being different from the origin of {I}. Some examples of visual-inertial navigation methods model motion in {I} to eliminate this centrifugal term. In the calculation method disclosed herein using both known and unknown visual features with depth information, the step of converting unknown depth features from {C} to {I} is easy. Not (see below). As a result, the system disclosed herein instead models all of the motion in {C}. In practice, the camera and IMU are mounted close to each other to minimize the effect of this term.

  The IMU bias can be a slowly changing variable. As a result, the most recently updated bias for the motion integral is used. First, Equation 3 is integrated over time. Subsequently, a translation is obtained from the acceleration data using the resulting orientation twice with time for the time integration with Equation 4.

Bias Correction IMU bias correction can be applied by feedback from either the camera or laser (see 128, 138 in FIGS. 1 and 2, respectively). Each feedback term includes estimated piecewise motion over a short amount of time. These biases can be modeled to be constant during piecewise movement. Starting from Equation 3, b _ω (t) can be calculated by comparing the estimated orientation to the IMU integral. The updated b _ω (t) is used in one more integration to recalculate the translation and compare it to the estimated translation to calculate b _a (t).

  To reduce the effects of high frequency noise, a sliding window is used while maintaining a known number of biases. Non-limiting examples of the number of biases used in a sliding window can include 200 to 1000, with a recommended number of biases based on an IMU rate of 200 Hz being 400. A non-limiting example of the number of biases in the sliding window for an IMU rate of 100 Hz is 100 to 500, with a typical bias number value of 200. The average number of biases from the sliding window is used. In this implementation, the length of the sliding window functions as a parameter to determine the bias update rate. Although alternative methods of modeling bias are known in the art, implementations of the present disclosure are used 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 a camera or a laser or both a camera and a laser as needed. For example, when the camera does not function, the IMU bias can be corrected with the laser alone instead.

  To reduce the effects of high frequency noise, a sliding window can be used while maintaining a certain number of biases. In such cases, the average number of biases from the sliding window can be used. In such an implementation, the length of the sliding window functions as a parameter that determines the rate at which the bias is updated. In some cases, the bias can be modeled as a random walk and the bias updated by the optimization process. However, a non-standard implementation is preferred to keep the IMU processing in a separate module. This implementation favors dynamic reconfiguration of the system, ie, the IMU can be coupled to either a camera or a laser. When the camera does not function, the IMU bias can be corrected by the laser instead. As described, inter-module communication within a sequential modular system is used to fix the IMU bias. This communication allows to correct the IMU bias.

According to exemplary and non-limiting embodiments, IMU bias correction can be performed by utilizing feedback from either a camera or a laser. Each of the camera and laser includes estimated piecewise motion over a short amount of time. When calculating the bias, the methods and systems described herein model the bias to build during a piecewise motion. Starting with Equation 3, the method and system described herein can calculate b _ω (t) by comparing the estimated orientation to the IMU integral. The updated b _ω (t) is used in one more integration to recalculate the translation and compare it to the estimated translation to calculate b _a (t).

  In some embodiments, the IMU output includes an angular velocity that has a relatively constant error over time. The resulting IMU bias is related to the fact that the IMU will always have some difference from the ground truth. This bias can change over time. This bias is relatively stationary and not high frequency. The slide window described above is a designated period during which the IMU data is being evaluated.

  Referring to FIG. 5, a system diagram of the visual-inertial odometry subsystem is presented. The method combines vision with an IMU. Both vision and the IMU constrain the optimization problem of estimating piecewise motion. At the same time, the method associates depth information with visual features. Depth can be obtained from the laser point when the feature is located in an area where laser distance measurements can be obtained. Otherwise, depth can be calculated from triangulation using the previous estimated motion sequence. As a final option, the method may use features for which no depth is known by formulating the constraints in a different manner. This may be due to features that may not always have laser distance coverage, or triangulation due to not always being tracked long enough or being located in the camera motion direction Fits features that cannot. These three alternatives can be used alone or in combination to construct an aligned point cloud. In some embodiments, the information is discarded. Eigenvalues and eigenvectors can be calculated and used to identify and specify degradation in the point cloud. When degradation exists in a particular direction in the state space, the solution in this direction in this state space can be discarded.

Visual-Inertial Odometry Subsystem A block system diagram of the visual-inertial odometry subsystem is depicted in FIG. The optimization module 510 uses the pose constraints 512 from the IMU prediction module 520 with camera constraints 515 based on optical feature data with or without depth information for motion estimation 550. Depth map alignment module 545 may include alignment and depth association of tracked camera features 530 with depth information obtained from laser points 540. The depth map alignment module 545 may also incorporate the motion estimation 550 obtained from previous calculations. The method couples vision closely with the IMU. Each of these places constraints 512, 515 on the optimization module 510 that estimates the piecewise motion 550, respectively. At the same time, the method associates depth information with visual features as part of depth map registration module 545. Depth is obtained from the laser point when the feature is located in an area where laser distance measurements can be obtained. Otherwise, the depth is calculated from triangulation using the previous estimated motion sequence. As a final option, the method may use features for which no depth is known by formulating the constraints in a different manner. This is the case for features that do not have laser range coverage or that cannot be tracked long enough or cannot be triangulated due to not being located in the direction of camera movement.

と表記する。

は、システム状態を表す式１内の

及びｘとは異なることに注意されたい。２つの理由、すなわち、１）深度関連付けがある程度の処理量を要し、深度関連付けをキー−フレームのみにおいて計算することによって計算強度を低減することができること、及び２）深度マップはフレームｃにおいて取得可能ではない可能性があり、位置合わせは既存の深度マップに依存することからレーザ点を位置合わせすることができない可能性があることからキー−フレームにおける特徴部を深度に関連付けることができる。｛Ｃ_c｝における正規化特徴部は、

と表記する。 Camera Constrained Vision-Inertial Odometry is a key-frame based method. A new key-frame is determined 535 if a certain number of features are missing or if the image overlap falls below a certain ratio. In this case, the upper right letter l (l∈Z ⁺ ) can indicate the last key-frame, and c (c∈Z ⁺ ) and c> k can indicate the current frame. As disclosed above, the method combines features with known depth and features with unknown depth. Depth-associated features in key-frame l can be denoted as _Xl = [ _xl , _yl , _zl ] ^T in { _Cl }. Correspondingly, features with unknown depth can be replaced using normalized coordinates.

Notation.

Is the expression in the expression 1 representing the system state.

And x. There are two reasons: 1) depth association requires a certain amount of processing and the computational intensity can be reduced by calculating the depth association only in key-frames, and 2) depth map is acquired in frame c The features in the key-frame may be associated with depth, as it may not be possible and the laser point may not be able to be aligned because the alignment depends on existing depth maps. The normalized feature in {C _c } is

Notation.

及び

は、３×３の回転行列及び３×１の平行移動ベクトルとし、この場合

∈ＳＯ（３）及び

∈

が成り立ち、

及び

はＳＥ（３）変換を形成する。フレームｌとｃの間の運動関数を次式のように書くことができる。
式（５）

as well as

Is a 3 × 3 rotation matrix and a 3 × 1 translation vector. In this case,

∈SO (3) and

∈

Holds,

as well as

Form the SE (3) transform. The motion function between frames l and c can be written as:
Equation (5)

が成り立つ。

を代入し、式５内の１番目及び２番目の行を３番目の行と組み合わせてｄ_cを排除すると以下に続く式を生じる。
式（６）

式（７）

_Xc has an unknown depth. When a d _c and depth,

Holds.

Substituting, it produces an expression that follows and to eliminate the d _c in combination with the third row of the first and the second row in the equation 5.
Equation (6)

Equation (7)

及び

のｈ番目の行である。特徴部に対して深度が取得不能である時に、ｄ_lをキー−フレームｌにおける未知深度とする。Ｘ_l及びＸ_cにそれぞれｄ_k

及びｄ_c

を代入し、式５内の３つ全ての行を組み合わせてｄ_k及びｄ_cを排除し、別の制約を生じる。
式（８）

R (h) and t (h) (h {1,2,3}) are

as well as

Is the h-th row. When the depth is impossible obtained for features, the d _l keys - the unknown depth of the frame l. D _{k for} X _l and X _c respectively

And d _c

, And combining all three rows in Equation 5 eliminates d _k and d _c , creating another constraint.
Equation (8)

をフレームａとｂとの間の次式の４×４の変換行列として定義することができる。
式（９）

The motion estimation motion estimation process 510 includes: 1) from features with a known depth, such as those in Equations 6-7, and 2) from features with an unknown depth, such as those in Equation 8. And 3) it is necessary to solve an optimization problem that combines three sets of constraints: those from IMU prediction 520.

Can be defined as the 4 × 4 transformation matrix between frames a and b:
Equation (9)

及び

は、対応する回転行列及び平行移動ベクトルである。更に

は、指数マッピングを通じて

に対応する３×１のベクトルとし、この場合、

∈ｓｏ（３）が成り立つ。正規化項θ／｜｜θ｜｜は、回転の方向を表し、｜｜θ｜｜は回転角である。各

は、カメラの６ＤＯＦ運動を含む

と

とのセットに対応する。

as well as

Is the corresponding rotation matrix and translation vector. Further

Is through exponential mapping

And a 3 × 1 vector corresponding to

∈so (3) holds. The normalization term θ / || θ || represents the direction of rotation, and || θ || is the rotation angle. each

Includes the 6DOF motion of the camera

When

Corresponding to the set.

を用いてＩＭＵ姿勢制約を定式化することができる。

で表記する最後の２つのフレームｃ−１とｃの間の予測変換は、ＩＭＵ機械化から取得することができる。フレームｃにおける予測変換は次式のように計算される。
式（１０）

The motion transformation between the determined frame 1 and c-1;

Can be used to formulate the IMU attitude constraint.

The predictive transformation between the last two frames c-1 and c, denoted by, can be obtained from IMU mechanization. The predictive transform in frame c is calculated as follows.
Equation (10)

及び

を

に対応する６ＤＯＦ運動とする。ＩＭＵ予測平行移動

は、方位に依存することが認められるであろう。例として、方位は、式（４）内の回転行列

を通じての重力ベクトルの投影、従って、積分される加速度を決定することができる。

は、

の関数として定式化することができ、

としてＩＭＵ予測モジュール１２２（図１及び図２）によって供給される２００Ｈｚの姿勢、並びに視覚−慣性オドメトリモジュール１２６（図１及び図２）によって供給される５０Ｈｚの姿勢は両方共に姿勢関数であることが認められるであろう。

を計算する段階は、フレームｃにおいて開始することができ、加速度は、時間に関して逆に積分することができる。

は、式（５）内にある

に対応する回転ベクトルとし、

及び

が求めるべき運動である。制約は、次式のように表すことができる。
式（１１）

式中の

は、適切にカメラ制約に対して姿勢制約をスケール調整する相対共分散行列である。

as well as

To

6DOF motion corresponding to. IMU prediction translation

Will be dependent on the orientation. As an example, the bearing is the rotation matrix in equation (4)

Of the gravitational vector through and thus the acceleration to be integrated can be determined.

Is

Can be formulated as a function of

The 200 Hz pose provided by the IMU prediction module 122 (FIGS. 1 and 2) and the 50 Hz pose provided by the visual-inertial odometry module 126 (FIGS. 1 and 2) may both be pose functions. Will be recognized.

Can start at frame c, and the acceleration can be integrated inversely with respect to time.

Is in equation (5)

And the rotation vector corresponding to

as well as

Is a movement that should be sought. The constraint can be expressed as:
Equation (11)

In the formula

Is a relative covariance matrix for appropriately scaling the pose constraint with respect to the camera constraint.

及び

を含む。従って、フルスケールＭＡＰ推定は実施されず、周囲問題を解くためにのみ用いられる。ランドマーク位置は最適化されず、従って、状態空間内では６つの未知数しか用いられず、それによって計算強度が低く保たれる。従って、本方法は、特徴部に精確な深度情報を与えて運動推定精度を保証するためのレーザ距離測定を含む。その結果、一括調節によって特徴部の深度の更に別の最適化を不要とすることができる。 In an embodiment of the visual-inertial odometry subsystem, the pose constraint satisfies the motion model and the camera constraint satisfies the landmark measurement model in equation (2). The optimization problem can be solved by using Newton's gradient descent method adapted to a robust fitting framework for the removal of outliers. In this problem, the state space is

as well as

including. Therefore, full-scale MAP estimation is not performed and is only used to solve the surrounding problem. The landmark positions are not optimized, so only six unknowns are used in the state space, thereby keeping the computational strength low. Accordingly, the method includes laser ranging to provide accurate depth information for the features to ensure motion estimation accuracy. As a result, further optimization of the depth of the feature can be made unnecessary by the collective adjustment.

Depth-associated depth map alignment module 545 aligns the laser point with the depth map using previously estimated motion. The laser point 540 inside the camera field of view is kept for a certain amount of time. The depth map is downsampled to maintain 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 two corner coordinates. Features can be projected onto this sphere when associating depth with features. Three laser points are determined on this sphere for each feature. In this case, the validity of these points may be by calculating the distance between these three points in the Cartesian space. When the distance is greater than the threshold, the point is likely to be from a different object, for example, a wall and an object in front of it, and the validation fails. Finally, the depth is interpolated from the three points, assuming a local planar patch in Cartesian space.

  Features without laser range coverage can be triangulated using the image sequence in which these features are tracked when they have been tracked over a certain distance and not located in the direction of camera movement. . In such a procedure, the depth can be updated in each frame based on the Bayesian probability mode.

Scan Matching Subsystem This subsystem further refines motion estimation from previous modules by laser scan matching. FIG. 6 depicts a block diagram of the scan matching subsystem. The subsystem receives the laser points 540 that are in the local point cloud and aligns these points using the supplied odometry estimate 550. Subsequently, geometric features from the point cloud are detected 640 and matched to the map. Scan matching minimizes the distance from the feature to the map, as well as many methods known in the art. However, odometry estimation 550 also provides pose constraints 612 in optimization 610. The optimization includes processing a pose constraint on the determined feature correspondence 615, which is further processed using a laser constraint 617 to generate a device pose 650. This attitude 650 is processed by the map alignment processing 655, which makes it easier to find the feature corresponding part 615. This implementation uses a voxel representation of the map. Further, the implementation can be dynamically configured to execute in parallel on one or more CPU threads.

Upon receiving a laser constrained laser scan, the method first aligns points from scan 620 in a common coordinate system. M (m∈Z ⁺ ) can be used to indicate the scan number. It should be understood that a camera coordinate system can be used for both the camera and the laser. Scan m can be associated with the camera coordinate system at the beginning of the scan, denoted as {C _m }. To locally align 620 the laser points 540, odometry estimates 550 from visual-inertial odometry can be taken as key points and IMU measurements can be used to interpolate between key points. .

Let P _{m be} the local registration point cloud from scan m. From P _m , two sets of geometric features, one on the sharp edge, in other words the edge denoted as ε _m, and the other on the local plane, in other words H _m The plane points to be described can be extracted. This extraction is based on the calculation of the curvature in the local scan. Points that have already selected neighbors, such as points that are on the boundary of the occluded area and that have local planes that are substantially parallel to the laser beam, are not employed. These points are likely to contain loud noise or change position over time as the sensor moves.

Subsequently, the geometric features are registered against the constructed current map. Q _m−1 is the map point cloud after processing the last scan, and Q _m−1 is defined by {W}. The points in Q _m-1 are separated into two sets, each containing an edge point and a plane point. Voxels can be used to store a map clipped at a certain distance around the sensor. For each voxel, two 3D KD trees can be constructed, one for edge points and the other for planar points. By using a KD tree for individual voxels, given a query point, point search can be performed because only the particular KD tree associated with a single voxel needs to be searched (see below). Speed up.

式中の_Xmは、ε_m又はＨ_m内の点であり、θ_m（θ_m∈ｓｏ（３））及びｔ_m（ｔ_m∈

）は、｛Ｗ｝における｛Ｃ_m｝の６ＤＯＦ姿勢を示す。 When aligning the scans, first ε _m and H _m are projected with the best motion guess available in {W}, then for each point in ε _m and H _m From the corresponding set on the map. To verify the geometric distribution of the point set, the associated eigenvalues and eigenvectors can be examined. In particular, one large eigenvalue and two small eigenvalues indicate an edge segment, and two large eigenvalues and one small eigenvalue indicate a local planar patch. When the match is valid, an equation for the distance from the point to the corresponding point set is formulated.
Equation (12)

_Xm in formula is a point in the epsilon _m or _{H m, θ m (θ m} ∈so (3)) and t _m (t _m ∈

) Indicates a 6 DOF posture of {C _m } in {W}.

は、オドメトリ推定によって与えられる｛Ｃ_m-1｝から｛Ｃ_m｝への姿勢変換とする。式（１０）と同様に、｛Ｗ｝における｛Ｃ_m｝の予測姿勢変換は次式で与えられる。
式（１３）

The motion estimation scan match is formulated into an optimization problem 610 that minimizes the overall distance described by equation (12). The optimization further includes the pose constraints 612 from the previous exercise. T _m-1 is a 4 × 4 transformation matrix for the orientation of {Cm-1} in {W}, where T _m-1 is generated by processing the last scan.

Is the posture transformation from {C _{m -1} } given by odometry estimation to {C _m }. Similarly to Expression (10), the predicted attitude conversion of {C _m } in {W} is given by the following expression.
Equation (13)

及び

は、

に対応する６ＤＯＦ姿勢とし、

は、相対共分散行列とする。制約は次式で与えられる。
式（１４）

as well as

Is

6DOF attitude corresponding to

Is a relative covariance matrix. The constraint is given by:
Equation (14)

及び

を用いることができる。加速度の積分は、方位（式（１１）における

と同じ）に依存することから

は、θ_mの関数である。ＩＭＵ姿勢制約は次式で与えられる。
式（１５）

式中の

は、対応する相対共分散行列である。最適化問題では、式（１４）及び式（１５）は、１つの制約セットへと線形結合される。この線形結合は、視覚−慣性オドメトリの作動モードによって決定される。ロバストな当て嵌めフレームワークに適応されたニュートン勾配降下法によって解かれる最適化問題は、θ_m及びｔ_mを精緻化する。 Equation (14) implies the case where the previous motion is from visual-inertial odometry, assuming the camera is in a functional state. Otherwise, the constraint is from IMU prediction. To represent the same term by IMU mechanization

as well as

Can be used. The integral of the acceleration is the bearing (in equation (11)

The same)

Is a function of θ _m . The IMU attitude constraint is given by the following equation.
Equation (15)

In the formula

Is the corresponding relative covariance matrix. In the optimization problem, equations (14) and (15) are linearly combined into one constraint set. This linear combination is determined by the mode of operation of the visual-inertial odometry. Optimization problem to be solved by the robust fitting Newton gradient descent which is adapted to the framework refine theta _m and t _m.

及び

を所与として、マップ上でセンサと一緒に移動する対応するＳ_m-1が存在する。センサがマップの境界に近づくと、境界の反対側にあるボクセル７２５がマップ境界７３０を拡張するように移動される。移動されたボクセル内の点は消去され、マップの切り取りが生じる。 Voxel Map Points on the map are kept in voxels. A two-level voxel implementation is illustrated in FIGS. 7A and 7B. Μ _m-1 represents the set of voxels 702, 704 on the first level map 700 after processing the last scan. Voxel 704 surrounding the sensor 706 forms a subset Micromax _m-1, denoted as S _m-1. 6DOF sensor attitude

as well as

, There is a corresponding S _m-1 that moves with the sensor on the map. As the sensor approaches the map boundary, voxels 725 on the other side of the boundary are moved to extend the map boundary 730. Points in the moved voxel are erased, resulting in clipping of the map.

と表記するボクセルセットによって形成される。走査を整合させる前に、最良の運動推測を用いてε_m及びＨ_m内の点がマップ上に投影され、

（

）内に埋められる。ε_m及びＨ_mからの点で占有されたボクセル７０８は抽出されてＱ_m-1が形成され、走査整合のための３Ｄ ＫＤ木内に格納される。ボクセル７１０は、ε_m及びＨ_mからの点で占有されない。走査整合の完了時に、走査は、マップをなすボクセル７０８内に融合される。その後、マップ点は一定の密度を維持するようにダウンサイズされる。第１のレベルのマップ７００の各ボクセルが第２のレベルのマップ７５０の部分ボクセルよりも大きい空間容積に対応することは認識することができる。従って、第１のレベルのマップ７００の各ボクセルは、第２のレベルのマップ７５０内の複数の部分ボクセルを含み、第２のレベルのマップ７５０内の複数の部分ボクセル上にマッピングすることができる。 As illustrated in FIG. 7B, each voxel j (jε∈S _m-1 ) of the second level map 750 has a smaller magnitude than that of the first level map 700.

Is formed by a voxel set denoted by Before aligning the scans, the points in ε _m and H _m are projected onto the map using the best motion guess,

(

) Is buried inside. Voxels 708 occupied at points from ε _m and H _m are extracted to form Q _m−1 and stored in a 3D KD tree for scan matching. Voxel 710 is not occupied in terms of ε _m and H _m . At the completion of the scan alignment, the scans are merged into the voxels 708 that make up the map. Thereafter, the map points are downsized to maintain a constant density. It can be appreciated that each voxel of the first level map 700 corresponds to a larger volume of space than the partial voxels of the second level map 750. Accordingly, each voxel of the first level map 700 includes multiple partial voxels in the second level map 750 and can be mapped onto multiple partial voxels in the second level map 750. .

（

）に対応するボクセルは、走査整合のためのセンサの周囲のマップを取り出すために用いられる。マップは、センサがマップ境界に近づく時にのみ切り取られる。従って、センサがマップの内部を進む時に切り取りは必要ない。別の注目点は、一方が縁点に対し、もう一方が平面点に対する２つのＫＤ木がＳ_m-1内の各個別ボクセルに対して用いられる点である。上述したように、そのようなデータ構造は点検索を高速化することができる。この方式で、

（

）内の各個別ボクセルに対して２つのＫＤ木を用いるのとは対照的に複数のＫＤ木の間の検索が回避される。

（

）内の各個別ボクセルに対して２つのＫＤ木を用いる場合は、ＫＤ木の構築及び保守のためにより多くのリソースを必要とする。 As described above with respect to FIGS. 7A and 7B, two voxel levels (first level map 700 and second level map 750) are used to store map information. The voxel corresponding to M _m-1 is used to maintain the first level map 700 and in the second level map 750

(

The voxel corresponding to ()) is used to retrieve a map around the sensor for scan alignment. The map is clipped only when the sensor approaches the map boundary. Therefore, no clipping is required as the sensor moves through the map. Another point of interest is that two KD trees, one for edge points and one for planar points, are used for each individual voxel in S _m-1 . As mentioned above, such a data structure can speed up point searches. In this way,

(

The search between multiple KD trees is avoided, as opposed to using two KD trees for each individual voxel in).

(

If two KD trees are used for each individual voxel in parentheses), more resources are required for KD tree construction and maintenance.

（

）がマップを綿密に検索するのに役立つことに起因する。これらのボクセルなしでは、Ｑ_m-1内により多くの点が含まれてＫＤ木内に組み込まれる。また、各ボクセルに対してＫＤ木を用いることにより、処理時間は、Μ_m-1内の全てのボクセルに対してＫＤ木を用いる場合と比較して若干短縮される。 Table 1 compares CPU processing times using different voxel and KD tree configurations. Times are averaged from multiple datasets collected from different types of environments covering closed and open structured and vegetation areas. It can be seen that when using only one voxel level Μ _m-1 , the construction and query of the KD tree requires about twice as much processing time. This is the second voxel level

(

) Helps to search the map in depth. Without these voxels, more points would be included in Qm _{-1 and} included in the KD tree. Further, by using the KD tree for each voxel, the processing time is slightly reduced as compared with the case where the KD tree is used for all the voxels within Μ _m−1 .

(Table 1)
Table 1. Comparison of average CPU processing time in KD tree operation

Parallel scan matching involves building a KD tree and iteratively finding feature matches. This process takes time and occupies the main computation in the system. While one CPU thread cannot guarantee the desired update frequency, a multi-threaded implementation can address complex processing problems. FIG. 8A illustrates a case where the two matcher programs 812 and 815 are executed in parallel. Upon receipt of the scan, the manager program 810 arranges the scan to match the latest available map. In one example, consisting of a collective environment with multiple structures and multiple visual features, matching is slow and may not be completed before the arrival of the next scan. The two matchers 812 and 815 are called alternately. In one matching unit 812, P _m 813a, P _m-2 813b, and still another P _mk (for k = even number) 813n are connected to Q _m-2 813a, Q _m-4 813a, and further Q _mk (k = for even) 813n. Similarly, in the second matcher 815, P _{m + 1} 816a, P _m-1 816b, and yet another P _mk (for k = odd) 816n are converted to Q _m-1 816a, Q _m-3 816a, And yet another Q _mk (for k = odd) 816n. The use of this alternating process can provide twice the amount of time for the process. In an alternative consisting of a clean environment with few structural and visual features, the computation is light. In such an example (FIG. 8B), only a single matcher 820 may be called. Since the alternating process is not _{required, P m, P m-1} , but are aligned respectively _{Q m-1, Q m-} 2, ··· (827a, 827b, see 827N) and in turn . In general, only two threads may be required, but this implementation can be configured to use up to four threads.

Transform Integration The final motion estimation is the integration of the outputs from the three modules depicted in FIG. The 5 Hz scan matched output produces the most accurate map, while the 50 Hz visual-inertial odometry output and the 200 Hz IMU prediction are integrated for high frequency motion estimation.

Robustness The robustness of the system is determined by its ability to deal with sensor degradation. It is always assumed that the IMU is functioning reliably as the backbone in the system. Cameras are subject to dramatic lighting changes and can fail in dark / uneven environments or when there is significant motion blur (which causes loss of visual feature tracking). Lasers cannot handle unstructured environments, such as scenes occupied by a single plane. Alternatively, laser data may be degraded due to data sparseness due to extreme exercise. Such extreme exercise includes highly dynamic exercise. As used herein, "highly dynamic motion" means a substantially sudden rotational or linear displacement of the system or a continuous rotational or translational motion having a substantially large magnitude. The self-motion determination system of the present disclosure can operate in the presence of highly dynamic motion as well as in dark, uneven and unstructured environments. In some exemplary embodiments, the system of the present invention can operate while undergoing high rotational angular velocities, on the order of 360 degrees per second. In other embodiments, the system can operate at linear velocities less than or equal to 100 kph. In addition, these movements can be a combination of angular and linear movements.

Both the visual-inertial odometry module and the scan matching module formulate and solve the optimization problem according to equation (2). When a failure occurs, it corresponds to a degraded optimization problem, ie, the constraints in some directions of the problem are in a bad state, and noise dominates in determining the solution. In one non-limiting method, .lambda.1 associated with the problem, λ2, ···, and eigenvalues denoted _{λ6, v 1, v 2,} ···, be calculated and eigenvectors denoted as v ₆ it can. Since the state space of the sensor includes 6 DOF (six degrees of freedom), there are six eigenvalues / eigenvectors. Without loss of generality, v _1, v _2, can be rearranged ..., and v ₆ in descending order. Each eigenvalue describes how well the solution is in the direction of its corresponding eigenvector. By comparing the eigenvalue with the threshold, a direction in a good state in the state space can be separated from a deterioration direction. Let h (h = 0; 1,..., 6) be the number of sufficiently conditioned directions. Two matrices can be defined as:
Equation (16)

When solving an optimization problem, a non-linear iteration can be started using initial guesses. Using the continuous pipeline depicted in FIG. 2, IMU prediction provides an initial guess for visual-inertial odometry, and the output of visual-inertial odometry is taken as an initial guess for scan matching. For two additional modules (visual-inertial odometry and scan matching modules), let x be a solution and let Δx be an update of x in a nonlinear iteration, where Δx solves the linearized system equation Is calculated by: During the optimization process, instead of updating x in all directions, x can be updated only in a well-conditioned direction, keeping the initial guess at the degradation direction.
Equation (17)

は、ゼロ行列になり、先行モジュールの出力が保たれる。 In equation (17), the system solves for motion in coarse to fine order starting from IMU prediction, and the additional two modules determine / refine motion as much as possible. Refinement can include all of the 6DOF when the problem is in good shape. Alternatively, the refinement can include from 0 DOF to 5 DOF, when the problem is only partially good. When the problem is completely degraded,

Becomes a zero matrix, and the output of the preceding module is kept.

及び

は、視覚−慣性オドメトリモジュールからの固有ベクトルを含む行列を表し、

は、サブシステム内の十分に条件付けされた方向を表し、

は、劣化方向を表す。組み合わせ制約は次式で与えられる。
式（１８）

Returning to the pose constraints described in Equations (14) and (15), it will be appreciated that these two equations are linearly combined in the scan matching problem. As defined in equation (16),

as well as

Represents a matrix containing eigenvectors from the visual-inertial odometry module,

Represents a well-conditioned direction within the subsystem,

Indicates a deterioration direction. The combination constraint is given by the following equation.
Equation (18)

が成り立ち、式（１８）は、式（１４）にあるもののような視覚−慣性オドメトリからの姿勢制約で構成される。しかし、カメラデータが完全に劣化した時に、

はゼロ行列であり、式（１８）は、式（１５）によるＩＭＵ予測からの姿勢制約で構成される。 When the camera is functioning normally,

Holds, and equation (18) consists of pose constraints from visual-inertial odometry such as in equation (14). However, when the camera data is completely degraded,

Is a zero matrix, and equation (18) is composed of pose constraints from IMU prediction according to equation (15).

Camera Degradation Case Study As depicted in FIG. 9A, the IMU prediction 122 is well conditioned in the visual-inertial odometry problem when visual features are poorly available for visual-inertial odometry. 924 bypassing the visual-inertial odometry module 126, which is completely or partially represented by a dotted line depending on the number of directions. Subsequently, the scan alignment module 132 can locally align the laser points for scan alignment. The bypassing IMU prediction is subject to drift. The laser feedback 138 compensates for the camera feedback 128 and corrects for IMU velocity drift and bias only in directions where it cannot be obtained. In this case, camera feedback has a higher priority when the camera data is not degraded, as higher frequencies make the camera feedback more appropriate. When enough visual features are found, no laser feedback is used.

Laser Degradation Case Study As shown in FIG. 9B, when the scan match 132 has insufficient environmental structure to refine the motion estimation, the output of the visual-inertial odometry module 126 maps the laser points onto a map. 930 bypassing the scan alignment module completely or partially as noted by the dotted line for alignment. When there are well conditioned directions in the scan matching problem, the laser feedback includes motion estimates refined in these directions. Otherwise, the laser feedback is empty 138.

In a more complex example than the camera and laser degradation case study , both the camera and laser degrade at least to some extent. FIG. 10 depicts such an example. A vertical bar with six rows represents a 6DOF pose where each row is a DOF (degree of freedom) corresponding to the eigenvector in equation (16). In this example, visual-inertial odometry and scan matching each update the 3DOF of the motion, leaving the motion unchanged at the other 3DOF. IMU predictions 1022a-102f may include initial IMU prediction values 1002. The visual-inertial odometry updates 1004 some 3DOFs (1026c, 1026e, 1026f) to produce refined predictions 1026a-1026f. Scan matching updates 1006 some of the 3DOFs (1032b, 1032d, 1032f) to yield more refined predictions 1032a-1032f. Camera feedback 128 includes camera updates 1028a-1028f, respectively, and laser feedback 138 includes laser updates 1038a-1038f, respectively. Referring to FIG. 10, cells without shade (1028a, 1028b, 1028d, 1038a, 1038c, 1038e) do not include any update information from each module. All updates 1080a-1080f to the IMU prediction module are a combination of updates 1028a-1028f from camera feedback 128 and updates 1038a-1038f from laser feedback 138. In one or more of the degrees of freedom in which feedback can be obtained from both a camera (eg, 1028f) and a laser (eg, 1038f), a camera update (eg, 1028f) has a higher priority than a laser update (eg, 1038f). Can be provided.

及び

をフレームｃ−１とｃの間に視覚−慣性オドメトリによって推定された６ＤＯＦ運動とし、この場合、

及び

が成り立つ。

及び

は、時間内挿後に走査整合によって推定された対応する項とする。

及び

は、視覚−慣性オドメトリモジュールからの固有ベクトルを含む式（１６）に定義された行列とすることができ、この場合、

は十分に条件付けされた方向を表し、

は劣化方向を表す。

及び

を走査整合モジュールからの同じ行列とする。次式は、組み合わせフィードバックｆ_Cを計算する。
式（１９）

式中のｆ_V及びｆ_sは、カメラフィードバック及びレーザフィードバックを表し、以下に続く式で与えられる。
式（２０）

式（２１）

However, in practice, the visual-inertial odometry module and the scan matching module may run at different frequencies, and each may have its own degradation direction. The IMU message can be used to interpolate between poses from the scan match output. In this manner, a piecewise motion that is time aligned with the visual-inertial odometry output can be created.

as well as

Be the 6 DOF motion estimated by visual-inertial odometry between frames c-1 and c, where

as well as

Holds.

as well as

Is the corresponding term estimated by scan matching after time interpolation.

as well as

Can be the matrix defined in equation (16) containing the eigenvectors from the visual-inertial odometry module, where

Represents a well-conditioned direction,

Represents the direction of deterioration.

as well as

Is the same matrix from the scan matching module. The following equation calculates the combination feedback f _C.
Equation (19)

F _V and f _s in the equations represent camera feedback and laser feedback, and are given by the following equations.
Equation (20)

Equation (21)

及び

は、次式のようにｆ_Cのヌル空間に投影することができる。
式（２２）

Note that f _C only includes motions determined in subspaces of the state space. Exercise from IMU prediction, ie

as well as

Can be projected to the null space of f _C as follows:
Equation (22)

及び

を用いることができる。方位

は、ｂ_ω（ｔ）にしか関連しないが、平行移動

は、ｂ_ω（ｔ）とｂ_a（ｔ）の両方に依存する。バイアスは、次式を解くことによって計算することができる。
式（２３）

To represent IMU prediction motion is formulated as a function of b _omega (t) and b _a (t) Equations (3) The integration of equation (4)

as well as

Can be used. Bearing

Is only related to b _ω (t), but the translation

Depends on both b _ω (t) and b _a (t). The bias can be calculated by solving the following equation.
Equation (23)

及び

はゼロ行列である。相応に、ｂ_ω（ｔ）及びｂ_a（ｔ）は、ｆ_Cから計算される。劣化した時に、ＩＭＵ予測運動

及び

は、運動を求めることができない（例えば、図１０の組み合わせフィードバックの白色の行１０８０ａ）方向に用いられる。結果は、先に計算されたバイアスがこれらの方向に保たれることである。 When the system is functioning properly, f _C spans the state space, and in equation (22)

as well as

Is a zero matrix. Correspondingly, b _omega (t) and b _a (t) is calculated from f _C. IMU prediction movement when deteriorated

as well as

Is used in directions where motion cannot be determined (eg, white row 1080a of the combined feedback in FIG. 10). The result is that the previously calculated bias is kept in these directions.

Experiment
It was verified odometry and mapping sidewall system to the test two sensor sets using the scanner. In the first set of sensors, a Velodyne Rider ™ HDL-32E laser scanner was attached to a UI-1220SE monochrome camera and an Xsens® MTi-30 IMU. This laser scanner has a horizontal FOV of 360 ° and a vertical FOV of 40 ° and senses 0.7 million points / sec at a rotation speed of 5 Hz. The camera was configured for a resolution of 752 pixels by 480 pixels, a horizontal FOV of 76 ° and a frame rate of 50 Hz. The IMU frequency was set at 200Hz. In the second set of sensors, a Velodyne Rider ™ VLP-16 laser scanner was attached to the same camera and IMU. This laser scanner has a horizontal FOV of 360 ° and a vertical FOV of 30 ° and senses 0.3 million points / sec at a rotation speed of 5 Hz. Each sensor set was attached to a data collection vehicle running on each of the streets and off-road areas.

  Up to 300 Harris corners were tracked for both sensor sets. To evenly distribute the visual features, the image was separated into 5 x 6 identical subregions, each providing up to 10 features. In order to maintain the number of features in each subregion, a new feature was created when tracking lost a feature.

  This software is a Linux (R) running a robot operating system (ROS) on a laptop computer with a 2.6 GHz quad-core processor (two threads on each core, for a total of eight threads) and a built-in GPU. Run on the system. Visual Feature Tracking implemented two versions of software running on each of the GPU and CPU. Table 2 shows the processing time. The time spent by visual-inertial odometry (126 in FIG. 2) does not vary much with environment or sensor configuration. In the GPU version, visual-inertial odometry consumes about 25% of CPU threads running at 50 Hz. In the CPU version, visual-inertial odometry occupies about 75% of the threads. The first set of sensors results in a slightly longer processing time than the second set of sensors. This is due to the scanner perceiving more points and the program requiring more time to maintain a depth map and associate depth with visual features.

  Scan matching (132 in FIG. 2) is longer and consumes varying processing time in relation to the environment and sensor configuration. In the first set of sensors, scan matching occupies about 75% of the threads running at 5 Hz when operated in a structured environment. However, in a vegetation-rich environment, more points are aligned on the map and programs typically consume about 135% of the threads. In the second set of sensors, the scanner perceives fewer points. Scan matching module 132 uses about 50-95% of the threads, depending on the environment. The time spent by the IMU prediction (132 in FIG. 2) can be ignored compared to the other two modules.

Accuracy test A test was performed to evaluate the accuracy of the proposed system. In these tests, a first set of sensors was used. The sensor was mounted on an off-road vehicle running around the university campus. After 2.7 km of driving within 16 minutes, a campus map was constructed. The average speed over the test was 2.8 m / s.

(Table 2)
Table 2. Average CPU processing time using first and second sensor sets

  Estimated trajectories and aligned laser points were registered on satellite images to evaluate motion estimation drift over the test. In this case, the laser spot on the ground was manually removed. By aligning the orbit with the street on the satellite image, it was found that the upper limit of horizontal error was <1: 0 m. Also, comparing the buildings on the same floor revealed that the vertical error was <2: 0 m. This gives a total relative position drift that is <0: 09% of the travel distance at the end. It will be appreciated that accuracy cannot be guaranteed for the measurements, and therefore only the upper limit of position drift has been calculated.

  In addition, a more comprehensive test was performed with the same sensor mounted on a car. The passenger car ran on a structured road for a distance of 9.3 km. The path was through vegetation, bridges, hilly terrain, and busy streets, and eventually returned to the starting location. Elevation varied over 70 m along the path. Except waiting for the traffic light, the vehicle speed during the test was between 9-18 m / s. It has been found that buildings found at both the start point and the end point of the route are registered in two places. The two registrations occur due to motion estimation drift over the path length. 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 distance measurement was <20 m, resulting in a relative position error of <0: 22% of the travel distance at the end.

  Each module in the system contributes to the overall accuracy. FIG. 11 illustrates an estimated trajectory in the accuracy test. The first trajectory plot 1102 of the trajectory of the mobile sensor generated by the visual-inertial odometry system is using 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. The second trajectory plot 1104 is based on transferring IMU predictions from the IMU module 122 directly to the scan matching module 132, bypassing visual-inertial odometry. This configuration is similar to that depicted in FIG. 9A. The third trajectory plot 1108 for 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), with only a minimal amount of drift. do not have. The position errors of the first two configurations, the trajectory plots 1102 and 1104, are about four and two times larger, respectively.

  The first trajectory plot 1102 and the second trajectory plot 1104 can be considered as expected system performance when individual sensor degradation is encountered. If the scan match degrades (see FIG. 9B), the system degrades to the mode indicated by the first trajectory plot 1102. If the vision deteriorates (see FIG. 9A), the system degrades to the mode indicated by the second trajectory plot 1104. If none of the sensors has degraded (see FIG. 2), the system incorporates all of the optimization functions and produces a trajectory plot 1108. In another example, the system employs the IMU prediction as an initial guess, but can operate at the laser frequency (5 Hz). The system generates a fourth trajectory plot 1106. The resulting accuracy is only marginally better compared to the second trajectory plot 1104 using the IMU coupled directly to the laser past the visual-inertial odometry. This result shows that the functionality of the camera is not fully utilized when solving the problem with all constraints superimposed.

  Another accuracy test of the system involved a mobile sensor traveling at the original 1 × speed and accelerated 2 × speed. As we proceeded at 2x speed, every other frame of data for all three sensors was omitted, resulting in more extreme movement throughout the test. Table 3 shows the results. Three configurations were evaluated at each speed. At 2 × speed, the accuracy of the visual-inertial odometry configuration and the IMU + scan matching configuration compared to the accuracy at 1 × speed is reduced by a significant ratio of 0.54% and 0.38% of the travel distance. However, a complete pipeline reduces accuracy by a very small percentage of only 0.04%. This result indicates that the camera and laser compensate each other to maintain overall accuracy. This is especially true during extreme exercise.

  Table 3. Relative position error as a percentage of travel distance

(Table 3)
(The error at 1 × speed corresponds to the trajectory in FIG. 11)

  Referring to FIG. 12, an exemplary, non-limiting embodiment of an interactive information flow is shown. As illustrated, three modules, including 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. The data processing flow is from left to right and goes through each of the three modules, whereas the feedback flow is from right to left and corrects for IMU bias.

  Referring to FIGS. 13a and 13b, an exemplary, non-limiting embodiment of a dynamically reconfigurable system is shown. As illustrated in FIG. 13a, when visual features are insufficient for visual-inertial odometry, IMU prediction bypasses (partially) the visual-inertial odometry module to locally align the laser points. In contrast, as illustrated in FIG. 13b, when the environmental structure is insufficient for scan matching, the visual-inertial odometry output (partially) bypasses the scan matching refinement module to map the laser points onto the map. Align.

  Referring to FIG. 14, an exemplary, non-limiting embodiment of priority feedback for IMU bias correction is shown. As illustrated, the vertical bar represents a 6 DOF pose, and each row is one DOF. When degraded, starting from the left IMU prediction with all six lines labeled "IMU", the visual-inertial odometry updates the 3DOF so that the line is labeled "camera", followed by The scan match makes an update that causes the row to be labeled "Laser" in another 3DOF. Camera feedback and laser feedback are combined like a vertical bar on the left. Camera feedback has a higher priority, and the "laser" row from laser feedback is only filled in if there is no "camera" row from camera feedback.

（（図１５ｂ）の全てのボクセル

）によって形成される。走査整合の前に、レーザ走査は、最良の運動推測を用いてマップ上に投影することができる。走査からの点によって占有された

（

）内のボクセルをクロスハッチングで標記している。続いて走査整合のためのクロスハッチング表示ボクセル内のマップ点が抽出されて３Ｄ ＫＤ木内に格納される。 Referring to FIGS. 15a and 15b, exemplary and non-limiting embodiments of a two-layer voxel representation of a map are shown. A voxel M _m-1 on the map (all voxels in FIG. 15a) and a voxel S _m-1 surrounding the sensor (dot fill voxel) are illustrated. S _m-1 is a subset of M _m-1 . When the sensor approaches the map boundary, voxels on the other side (bottom row) of the boundary are moved to extend the map boundary. Points in the moved voxel are cleared and the map is clipped. As illustrated in FIG. 15b, each voxel j (j∈S _m-1 ) (dot-filled voxel in FIG. 15a) is a set of voxels with a small magnitude.

(All voxels in (FIG. 15b)

). Prior to scan matching, the laser scan can be projected on a map with the best motion guess. Occupied by points from the scan

(

The voxels in parentheses are marked with cross-hatching. Subsequently, map points in the cross-hatched display voxels for scan matching are extracted and stored in the 3D KD tree.

Referring to FIG. 16, an exemplary, non-limiting embodiment of a multi-threaded scan matching process is shown. As illustrated, the manager program calls multiple matcher programs running on separate CPU threads to match the scan to the latest map available. FIG. 16A shows the case of two threads. Scanning _{P m, P m-1,} ··· is, Q _m on each matcher, Q _m-1, is aligned with., The amount of time twice is given to processing. By comparison, FIG. 16b shows the case of one thread, where P _m , P _m−1 ,... Are matched with Q _m , Q _m−1 ,. This implementation is dynamically configurable with up to four threads.

  In embodiments, a real-time SLAM system can be used in combination with a real-time navigation system. In embodiments, the SLAM system can 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 heat-based system, or the like. This obstacle detection may include detecting an active obstacle, such as people, pets, or the like, by motion detection, heat detection, electric or magnetic field detection, or other mechanisms.

  In embodiments, short-range features, such as objects that provide a near reflection to the SLAM system, as well as long-range features, such as items that can be scanned through the space between the short-range features or through openings in the short-range features. A point cloud established by scanning a feature of the environment to show a spatial mapping, which can include part mapping, can be displayed on a screen, such as a screen forming part of a SLAM. . For example, items in adjacent corridors can be scanned through such spaces or openings as the mapper moves around the room because different outdoor elements can be scanned through windows or doors at various points in the room. In this case, the resulting point cloud may include comprehensive mapping data of the immediate environment and the partial mapping of distant elements outside of the environment. Thus, a SLAM system can include mapping a space through a “pile fence” by identifying a distant object through a space or opening (ie, a gap in the fence) that is within close range. The long-range data may be converted, for example, from a comprehensively mapped space (having a well-understood orientation and position due to the density of the point cloud) to a sparsely mapped space (such as a new room). A mapper that maintains a consistent location estimate when moving from space to space can be used to help the system orient the SLAM as it moves from space to space. As a user moves from a near location to a far location, the SLAM system uses the relative density or sparseness of the point cloud to guide the mapper, for example, through a user interface that forms part of the SLAM. For example, the mapper can be directed from another space to a distant portion that cannot be seen through the aperture.

  In embodiments, point cloud maps from the SLAM system can be combined with mappings from other inputs, such as cameras and sensors. For example, in the case of a flight or spacecraft, an airplane, drone, or other flying mobile platform can be used as reference data for a SLAM system (eg, linking a point cloud resulting from a scan to a GPS reference location), or Possibly already equipped with other distance measuring and geographic localization devices that can obtain from the scan reference data to display further scan data, for example as an overlay on the output from other systems There is. For example, a conventional camera output can be shown along with the point cloud data as an overlay, or vice versa.

  In an embodiment, the SLAM system can provide a point cloud that includes data indicating the reflection intensity of the return signal from each feature. This reflection intensity is used to determine the validity of the signal to the system, to determine how the features relate to each other, to determine the surface IR reflectance, and the like. Can be. In embodiments, this reflection intensity can be used as a reference for manipulating the point cloud display in the map. For example, a SLAM system can introduce some color contrast (automatically or under user control) to highlight the reflectivity of a signal for a given feature, material, structure, or the like. ). In addition, SLAM systems can be combined with other systems to extend color and reflection coefficient information. In embodiments, one or more of the points in the point cloud are displayed using colors corresponding to parameters of the acquired data, such as intensity parameters, density parameters, time parameters, and geospatial location parameters. Can be. The coloring of the point cloud may assist the user in understanding and analyzing elements or features of the environment in which the SLAM system operates and / or features of the acquisition process of the point cloud itself. it can. For example, using a density parameter to indicate the number of points acquired within a geospatial area, the color representing the area where many data points are acquired and the data being sparse and false instead of "real" data Another color can be determined to indicate the presence of the image. The color can also indicate, for example, the time that evolves through the sequence of colors as the scan is performed, resulting in a clear indication of the path through which the SLAM scan is performed. Coloring can be used to distinguish between various features (e.g., comparing furniture in a space to a wall), to provide an artistic effect, to highlight areas of interest (e.g., to focus the attention of a scanning observer). (For example, related devices are highlighted).

  In embodiments, the SLAM system may identify “shadows” (areas where the point cloud has relatively few data points from the scan) and may highlight areas that require additional scans. Yes (eg, through a user interface). For example, such areas may be drawn or painted in a particular color within the visual interface of a SLAM system displaying a point cloud until the shadow areas are fully "painted" or covered by laser scanning. Such an interface can include any indicator (visual, text-based, audio-based, or the like) to the user that highlights an area in the field of view that has not yet been scanned. Such an indicator can be used to obtain the user's attention directly or through an external device (such as the user's mobile phone). According to other exemplary and non-limiting embodiments, the system of the present invention provides a method for identifying previously unscanned areas on SLAMs such as previously constructed point clouds and maps for comparison with the current scan. Can refer to external data of data stored in.

  In embodiments, the methods and systems disclosed herein provide a map composed of point cloud data indicating environmental features to determine accurate position and orientation information or based on reflected signals from a laser scan. Include a SLAM system that provides real-time positioning output at operating points without requiring processing or calculations by an external system to generate the 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 the laser scan.

  In embodiments, the SLAM system may be a variety of external systems such as vehicle navigation systems (for unmanned vehicles, drones, mobile robots, unmanned underwater vehicles, self-driving vehicles, semi-automatic vehicles, and many others). Can be integrated with the system. In embodiments, a SLAM system may be used to allow a vehicle to travel within its own environment without resorting to external systems such as GPS.

  In an embodiment, the SLAM system may determine a confidence level with respect to a current estimate of position, heading, or the like. The confidence level may be based on the density of points obtainable in the scan, the orthogonality of points obtainable in the scan, the geometry of the environment, or other factors, or a combination thereof. The confidence level shall be attributable to an estimate of the position and orientation at each point along the scan route so that the segments of the scan can be referred to as low-reliability segments, high-reliability segments, or the like. Can be. Unreliable segments can be highlighted for additional scanning, use of other techniques (such as making adjustments based on external data), or the like. For example, if the scan is performed in a closed loop (the end point of the scan is the same as the start point at the known first location), any difference between the calculated end location and start location will be This can be solved by preferentially adjusting the location estimate for certain scan segments to restore location and end location coincidence. Location and location information within the unreliable segment can be adjusted preferentially compared to the unreliable segment. Thus, the SLAM system can use reliability-based error correction for closed-loop scanning.

In an exemplary and non-limiting embodiment of this reliability-based adjustment, a derivation of the piecewise smoothing and mapping (iSAM) algorithm (M. Kaess, A. A.) originally developed by Michael Kaess and Frank Delaert of Georgia Tech. Langanathan and F. Dellert, "iSAM: Incremental Smoothing and Mapping (iSAM)," IEEE Trans. On Robotics (TRO), Vol. 24, No. 6, December 2008, 1365 1378 page, PDF ). This algorithm processes the map data in "segments" and iteratively refines the relative positions of these segments to optimize the residual error of alignment between the segments. This allows the loop to be closed by adjusting all the data in a closed loop. Many segmentation points allow the algorithm to move data significantly, while less segmentation points result in higher stiffness.

  When points for segmentation are selected based on the matching reliability metric, this can be used to reduce the likelihood that the loop closure process will not distribute local errors to the correct map sections in regions with low reliability. The map can be flexible and rigid in highly reliable areas. This can be further improved by weighting the segmentation points to assign "flexibility" to each point and dispersing the error based on this factor.

  In embodiments, a confidence measure for an area or segment of a point cloud may be used to guide a user to perform additional scans, for example, to achieve an improved SLAM scan. In embodiments, the confidence measure may be based on a combination of point density, point orthogonality, etc., and may guide the user to enable a more accurate scan. Note that scanning attributes, such as point density and point orthogonality, can be determined in real time as the scan proceeds. Similarly, the system can sense the geometry of the scanning environment that is likely to produce a low confidence measure. For example, long corridors with smooth walls may not show any irregularities to distinguish one scan segment from the next. In such cases, the system may assign a lower confidence measure to scan data acquired in such an environment. The system determines the reduced reliability and the rider, camera, and possibly other sensors to guide the user by instructions (such as "slow down", "turn left", or the like) through scanning. Various inputs such as can be used. In other embodiments, the system displays a less reliable area than desired to the user, such as through a user interface, while allowing the user to further scan the less reliable area, volume, or area. You can provide assistance to

  In embodiments, the SLAM output can be fused with other content, such as output from a camera and output from other mapping techniques. In an embodiment, the SLAM scan may be performed in conjunction with capturing an audio track, such as in an attendant application (optionally a mobile application) that captures the time-encoded audio sound corresponding to the scan. In an embodiment, the SLAM system is configured to collect data during the scan so that the mapping system can accurately indicate when and where the scan occurred, including when and where the mapper acquired the audio and / or sound. Is applied. In an embodiment, the temporal encoding is such that sound is generated by inserting data into the map or scan that can be accessed by the user, such as by clicking on an indicator on the map where audio is available. It can be used to localize the sound within a significant map area. In embodiments, other media formats, such as photos, HD video, or the like, may be captured and synchronized with the scan. These media formats can be accessed separately based on time information, or inserted at appropriate places in the map itself based on the time synchronization of scan output with time information on other media. . In an embodiment, a user can go back in time using time data and see what has changed over time based on multiple scans or the like with different time-encoded data. Scanning can be further refined using other information such as date stamps or time stamped activity records. From the above, the scan can be part of a multi-dimensional database of scenes or space, where the point cloud data is other data or media related to this scene, including time-based data or media. Is associated with In an embodiment, the calculation allows, for example, to back up the scan so that it returns to a given point in the scan and resumes at that point instead of having to restart a new scan starting at the origin In a manner maintained throughout the sequence of steps or segments. This allows the use of partial scan information as a starting point to continue scanning, for example, when problems occur later in the scan that initially produced good output. In this case, the user can "discard" or "rewind" the scan until it returns to a point, and then resume scanning from this point. The system can maintain accurate location and location information based on point cloud features, and can also maintain time information to allow for temporal alignment with other time-based data. . The time-based data may be used to compile these edits, such as scanning or synchronizing other media, if the scan is completed over one time interval and needs to be synchronized with other media captured over different time intervals. It can be possible. Data in the point cloud can be tagged with a time stamp, thereby erasing the time-stamped data after the point to which it was rewound so that the scan could resume from the specified point. it can. In embodiments, rewinding may be to a physical location, such as rewinding to a point in time and / or to certain geospatial coordinates.

  Embodiments can fuse the output from a SLAM-based map with other content, such as HD video, including coloring the point cloud and using it as an overlay. This fusion can include time synchronization between the SLAM system and other media capture systems. Content may be fused with video, spatial still images, spatial CAD models, audio content captured during scanning, metadata associated with locations, or other data or media.

  In embodiments, the SLAM system can integrate with other technologies and platforms, such as tools (eg, CAD) that can be used to manipulate point clouds. This integration involves combining scans with features modeled in CAD modeling tools, rapid prototyping systems, 3D printing systems, and other systems that can use point clouds or solid model data. Can be. The scan can be provided as input to a post-processing tool, such as a coloring tool. Scanning can be provided to a mapping tool, for example, to add points of interest, metadata, media content, annotations, navigation information, semantic analysis to distinguish particular shapes and / or identify objects. .

  Output combined with output from other scanning and image capture systems such as underground radar, x-ray imaging, magnetic resonance imaging, computed tomography, infrared imaging, photography, video, SONAR, radar, lidar, etc. Can be. This combination may include integrating the output of the scan with a navigation and mapping system such as an on-board navigation system, a hand-held mapping system, a mobile phone navigation system, and others. The data from the scan can be used to provide other systems with position and orientation data, including X, Y, and Z position information, as well as pitch, roll, and yaw information.

  Data acquired from real-time SLAM systems include, among others, 3D motion capture systems, acoustic engineering applications, biomass measurement, aircraft construction, archeology, architecture and civil engineering, augmented reality (AR), autonomous vehicles, autonomous mobile robot applications, Purification and treatment, CAD / CAM applications, construction site management (eg, progress monitoring), recreation, exploration (space, mining, underwater, etc.), forestry (logging and management of other forestry products such as maple sugar), franchises. Management and compliance (eg stores and restaurants), imaging applications for verification and compliance, indoor location, indoor design, inventory inspection, landscape design, industrial space mapping for maintenance, trucking route mapping, military / intelligence Applications, mobile mapping, oil pipeline monitoring and drilling, land and building assessment and other real estate applications, Store location (eg, combining real-time maps with inventory maps), security use, stock monitoring (ore, wood, supplies, etc.), assessment (including pre-scrutiny to make better estimates) ), UAV / drone, mobile robot, underground mapping, 3D model creation, virtual reality (including space coloring), warehouse management, district system / planning, autonomous mission, inspection, docking (spacecraft and ships) ), Cave exploration, and video game / augmented reality applications. In all usage scenarios, the SLAM system can operate as described herein in the fields of use mentioned.

  According to an exemplary and non-limiting embodiment, the unit includes hardware synchronization of the IMU, camera (vision) and LiDAR sensor. The unit can be operated for a period of time in a dark or unstructured environment. The processing pipeline can be composed of modules. In the dark, the visual module can be bypassed. In an unstructured environment, the LiDAR module can be bypassed or partially bypassed. In an exemplary, non-limiting embodiment, all IMU, rider, and camera data is time stamped and these data can be time aligned and synchronized. As a result, the system can function in an automated manner to synchronize the image data with the point cloud data. In some cases, the chunk data pixels may be colored for display to a user using color data from the synchronized camera image.

  The unit may include four CPU threads for scan alignment, for example, running at 5 Hz with Velodyne data. The movement of the unit in operation can be relatively fast. For example, the unit can operate at an angular velocity of about 360 degrees / second and a linear velocity of about 30 m / s.

  The unit can be localized relative to the previous occurrence map. For example, instead of building a map from scratch, the unit's software may refer to a previously built map and use the sensor pose and new map within this old map framework (eg, geospatial or other coordinates). Can be generated. The unit can further extend the map using localization. By deploying the new map within the old map frame, the new map can be further expanded and popped out of the old map. Thereby, an initial "backbone" scan is first generated and post-processed in some cases to reduce drift and / or other errors, and then resumed from this map to resume local details such as side rooms in the building A variety of modes of use are possible, including branching and chaining, which add to or increase point density in the region of interest. By employing this scheme, the backbone model can be generated with special care to limit global drift, and subsequent scans can be concentrated on capturing local details. Also, multiple devices for faster large area capture can perform detailed scans starting from the same basic map.

  In addition, it is possible to start with a model generated by a different type of device or to restart from a model generated from a CAD drawing. For example, a fixed device with high global accuracy can build a base map, and a mobile scanner can resume from this map and fill in details. In an alternative embodiment, a long-range device can scan outside and large interior areas of a building, and a short-range device can resume from this scan and add smaller hallways and rooms and more detailed details. it can. Resuming from a CAD drawing can have a great advantage in terms of quickly detecting the difference between the CAD and the one at the completion of the construction.

  The restart may also provide location aligned time data. For example, multiple scans can be obtained over time from the construction site to visually confirm progress. In other embodiments, multiple scans of the factory may aid tracking for asset management.

  Resume can alternatively be used simply to provide localization data within the scope of the previous map. This can be useful in guiding a robotic vehicle or in localizing new sensor data, such as images, thermal maps, sounds, etc., within existing maps.

  In some embodiments, the unit uses a relatively high CPU usage in the mapping mode and a relatively low CPU usage in the localization mode suitable for long-term localization / navigation. In some embodiments, the unit supports long-term operation by performing an internal reset from time to time. This internal reset is beneficial because some of the values generated during internal processing increase over time. Over a long period of operation (e.g., several days), these values can reach limits, such as logical or physical limits on storage for the values in the computer, and possibly cause processing to fail without a reset. is there. In some cases, the system can automatically flush the RAM memory to improve performance. In other embodiments, the system may downsample old scan data, as may be necessary when performing a real-time comparison of newly acquired data with old and / or archived data. it can.

  In other example embodiments, the unit may support flight applications and aerial map-ground map fusion. In another embodiment, the unit may calculate the attitude output at the IMU frequency, for example, 100 Hz. In such cases, the software can generate the map as well as the sensor pose. The sensor pose conveys the position and orientation of the sensor with respect to the map being developed. High frequency, accurate attitude output is useful for autonomous movement as vehicle motion control requires such data. The unit can further use covariance and estimated reliability to fix the attitude when the sensor is stationary.

  Referring to FIGS. 17 (a)-(b), exemplary and non-limiting embodiments of a SLAM are shown. The rider is rotated to generate a substantially hemispherical scan. This rotation is performed by a mechanism having a DC motor that drives spur gear reduction to the rider mount by spur gear assembly 1704. Spur gear reduction assembly 1704 allows the rider to be offset from motor 1708. A slip ring exists along the rider's rotation to power and receive data from the spinning rider. An encoder 1706 for recording the orientation of this mechanism during scanning is also coincident with the rider's rotation. Thin section contact bearings support the rider's rotating shaft. A counterweight on the rider's rotating plate balances the weight around the axis of rotation and makes the rotation smooth and constant. As illustrated in the rider drive mechanism and mounting diagrams below, this mechanism has minimal sway and rattling to allow for maintaining a constant speed for interpolation of scan point locations. Not designed. Note that motor shaft 1710 is in physical communication with rider connector 1712.

  Referring to FIG. 18, an exemplary, non-limiting embodiment of a SLAM enclosure 1802 is shown. SLAM enclosure 1802 is depicted in various figures and perspective views. Dimensions represent an embodiment and may be similar or different in size while maintaining the general features and orientation of key components such as riders, odometry cameras, tinted cameras, and user interface screens, etc. It is non-limiting because it can be done.

  In some embodiments, the unit may be, for example, a neck brace, a shoulder brace, a carry bag, or other wearable element or device (not shown) to assist an individual in holding the unit while walking around Can be used. The unit or support element or device may include one or more stabilizing elements to reduce wobble or vibration during scanning. In other embodiments, the unit can use a remote battery carried in a shoulder bag or the like to reduce the weight of the hand-held unit, so the scanning device has an external power supply.

  In other embodiments, the cameras and riders are arranged to maximize the field of view. The camera-laser arrangement creates a compromise. On the one hand, the camera blocks the laser FOV, and on the other hand the laser blocks the camera. In such an arrangement, both of these receive some occlusion, but this occlusion does not significantly compromise mapping quality. In some embodiments, the camera is pointed in the same direction as the laser because the visual processing is assisted by the laser data. Laser distance measurement provides depth information to image features during processing.

  In some embodiments, a reliability metric that represents the reliability of the spatial data may be used. Such reliability metric measurements can include, but are not limited to, the number of points, distribution of points, orthogonality of points, environmental geometry, and the like. One or more reliability metrics for laser data processing (eg, scan matching) and image processing can be calculated. Referring to FIGS. 19 (a) -19 (c), illustrative and non-limiting examples showing point clouds distinguished by laser match estimation reliability are shown. In practice, such images can be color coded, but as illustrated both trajectories and points are drawn as solid or dotted lines within the group based on the last reliability value at the time of recording. . In these examples, dark gray is bad and light gray is good. The values are thresholded so that everything with a value> 10 is a solid line. Experiments have shown that those with Velodyne <1 are not reliable, <10 is unreliable, and> 10 is very good.

  The use of these metrics allows for automated testing to solve model problems and off-line model corrections, for example, when utilizing the loop closure tool described elsewhere herein. In addition, the use of these metrics allows the user to be notified when a match is bad, possibly auto-pause, discarding unreliable data, and notifying 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 fast pace. Note that there is a slight spread of dust when compared to a scan taken from a slower scan pace, due in part to the speed at which the scan is performed. FIG. 19 (a) illustrates a display in which a potential defect spot having relatively low reliability is zoomed in.

  Referring to FIG. 20, an exemplary, non-limiting embodiment of a scan-to-scan match reliability metric process is illustrated wherein a visual feature follows between a full laser scan and a map constructed from a previous full laser scan. The average number of copies can be calculated and presented visually. This metric can provide a useful but different measure of reliability. In FIG. 20, a laser scanning reliability metric diagram is presented in the left pane, and the average number of visual feature metrics for the same data is presented in the right pane. As before, the dark gray line indicates low reliability and / or low average number of visual features.

  In some embodiments, loop closure can be used. For example, the unit can be activated when walking around a room, a private room, entering and exiting an office, and returning to a starting point. Ideally, the data mesh from the start and end points must be meshed accurately. In reality, there may be some drift. The algorithm described herein significantly reduces such drift. Typical reductions are of the order of 10 × over conventional methods (0.2% vs. 2%). This ratio reflects the error in the distance between the start and end points divided by the total distance traveled during the loop. In some embodiments, the software can recognize that it has returned to the starting point and re-fix to the origin. Once completed, the variability can be captured and spread over all of the collected data. In other embodiments, certain point cloud data may be determined where the reliability metric indicates that the data reliability was poor, and adjustments may be made to areas with low reliability.

  In general, the system of the present invention can use explicit and implicit loop closure. In some cases, the user may indicate that the loop should be closed, such as through a user interface that forms part of the SLAM. As a result of this explicit loop closure, the SLAM converts the most recent scan data to the data acquired at the beginning of the loop in order to close the loop with the data acquired at the beginning and the data acquired at the end. Can be matched. In other embodiments, the system of the present invention can implement implicit loop closure. In such cases, the system of the present invention can operate in an automated manner, recognizing that it will actively rescan the location including the origin or original area for the scan loop. .

  In some embodiments, a reliability-based loop closure can be used. First, the starting and ending points of a loop scan of an area containing a plurality of segments can be determined. Next, the reliability of the plurality of segments can be determined. Subsequently, adjustments can be made to the lower quality segments rather than the higher quality segments to match the beginning and end of the loop.

  In another exemplary embodiment, a multi-loop reliability-based loop closure can be implemented. In still other embodiments, a reliability-based loop closure that provides semantic adjustment may be used. For example, structural information can be derived from the attributes of the scanning target element, that is, the floor is flat, the corridor is straight, and the like.

  In some cases, coloring of the rider point cloud data can be used. In some embodiments, coarse coloring of collection points can be used in real time to help identify what has been captured. In other embodiments, off-line photorealistic coloring can be used. In other embodiments, each pixel in the camera can be mapped to a unique lidar pixel. For example, color data can be obtained from a pixel in a colored camera corresponding to the rider data in the point cloud and added to the rider data.

  According to an exemplary and non-limiting embodiment, the unit can use a sequential multi-layer processing pipeline that solves for motion from coarse to fine. At each layer of the optimization, the previous coarse result is used as an initial guess for the optimization problem. The stages in the pipeline are as follows:

  1. Start with an IMU mechanism for motion prediction that performs high frequency updates (around 200 Hz) but experiences high levels of drift.

  2. This estimate is then refined by visual-inertial odometry optimization at the camera frame rate (30-40 Hz), which uses the IMU motion estimation as an initial guess of pose changes and tracks from the current camera frame. This pose change is adjusted in an attempt to minimize the residual squared error in the movement between some features to the keyframe.

  3. Subsequently, this estimate is further refined by laser odometry optimization at a lower speed determined by the "scan frame" speed. The scan data arrives continuously and the software divides the data into a number of frames, similar to image frames, at a constant speed, at which point a single rotation of the rider rotation mechanism is used for each scan. This is the time it takes to make a frame into a complete data hemisphere. These data are glued together using visual-inertial estimation of position changes since points in the same scan frame are collected. In the lidar odometry pose optimization phase, the visual odometry estimate is taken as an initial guess, and the optimization attempts to reduce residual errors in features tracked in the current scan frame that are aligned with the previous scan frame.

  4. In the final stage, the current scan frame is aligned with the entire previous map. Taking the laser odometry estimate as an initial guess, the optimization minimizes the residual square error between features in the current scan frame and features in the previous map.

  The resulting system allows for high frequency, low latency, self-motion estimation in conjunction with dense, accurate 3D map registration. Further, the system can address sensor degradation by automatic reconfiguration around failed modules, as each stage can correct for errors in previous stages. Thus, the system can operate in the presence of highly dynamic movements and in an environment without dark irregularities and structures. During the experiment, the system demonstrated 0.22% relative position drift over 9.3 km of navigation and was robust for running, jumping, and even for high speed driving on highways (up to 33 m / s). Demonstrated the nature.

  Other key features of such a system may include:

Optimization of visual features with and without depth : The software first attempts to correlate the depth of the tracked visual features with the laser data and then triangulates the depth between camera frames. You can try to decide by trying. The feature optimization software can then utilize all features in two different error calculations, one for features with known depth and one for features with unknown depth.

Laser feature determination : The software can extract laser scan features as the scan line data arrives rather than the entire scan frame. This extraction is much simpler, examining the smoothness at that point, defined by the relative distance between each point and the K nearest points on either side, followed by the smoothest point Is classified as a plane feature, and the sharpest point is classified as an edge feature. This extraction also allows for the elimination of some points that can be bad features.

Map matching and voxelization : Part of how laser matching works in real time is how the map and feature data are stored. Tracking the processor load of this stored data to minimize the data stored while retaining what is required for accurate alignment is a long-term scan and selective voxelization to three-dimensional basic units. Or it is important for downsampling. This downsampling voxel size or on-the-fly adjustment of the base unit based on processor load can improve the ability to maintain real-time performance on large maps.

Parallel processing : The software can be configured such that when data arrives faster than the processor can process the data, parallel processing can be utilized to maintain real-time performance. This parallelism is more significant for fast point / second riders such as velodyne.

Robustness : An approach in which the system of the present invention uses a separate optimization stage (apart from being an initial guess) without including the previous estimate as part of the next estimate is somewhat intrinsically robust. Produces.

Reliability metric : Each optimization step in this process can provide information about the reliability of the result of that step itself. In particular, at each stage, the residual square error remaining after optimization and the number of features tracked between frames can be evaluated to provide a measure of reliability in the results.

  The user can be presented with a reduced (eg, partially sampled) version of the multispectral model given the data being acquired by the device. In one example, cube model data, each measuring 3 cm × 3 cm × 3 cm, can be represented as a single pixel in a reduced version presented on a user interface. The pixel selected for display may be the pixel closest to the center of the cube. A representative reduced display generated during operation of the SLAM is shown below. As described, the decision to display a single pixel in a volume depends on the presence of one or more points or the absence of any such points occupying a spatial cube of predetermined dimensions in the point cloud. Represents a binary result indicating either of the following. In another exemplary embodiment, the selected pixel may be attributed with a value indicating, for example, the number of pixels within the predetermined cube represented by the selected pixel. This attribute can be used when displaying the partially sampled point cloud, for example, by displaying each selected pixel using a color and / or intensity that reflects the value of this attribute.

  The visual frame contains a single 2D color image from a tinted camera. The rider segment includes a full 360 degree rotation of the rider scanner. The visual frame and rider segments can be combined to match existing model data based on unit position data captured from IMUs and associated sensors such as odometry (eg, high speed white / black) cameras. Synchronized.

  Issues that cause low reliability metrics include, but are not limited to, glossy walls, glass, clear glass, and reflections from narrow hallways. In some embodiments, the user of the unit pauses scanning, for example, by pressing a pause button and / or requesting rewinding to a predetermined point or past requested seconds. , And can resume.

  According to an exemplary and non-limiting embodiment, rewinding during a scan can proceed as follows. First, indicate that the user of the system wants to rewind. This ideology can be achieved by manipulating the user interface that forms part of the SLAM. As a result of indicating that rewind is desired, the system deletes or otherwise removes portions of the scanned data points that correspond to a certain time span. Since all scanned data points are time stamped, the system can effectively remove the data points after the predetermined time and thus "rewind" to a previous point in the scan. As described herein, images from the camera are collected and time stamped during each scan. As a result, after removing the data points after the predetermined point in time, the system presents to the user an indication of the image recorded at the predetermined point while displaying the scanned point cloud rewinded to the predetermined point in time. be able to. The image serves as a metric to assist the user of the system in orienting the SLAM to a position that closely matches the orientation and attitude of the SLAM at a previous predetermined point in time. Once the orientation of the user has been determined to be close to the past orientation of the SLAM at the predetermined point in time, the user may indicate a desire to resume scanning, such as by pressing an "execute" button on the SLAM user interface. . In response to a command to resume scanning, the SLAM may proceed with executing a processing pipeline utilizing the new scan data to form an initial estimate of the SLAM's position and orientation. During this process, the SLAM may not add new data to the scan, but with the new scan data the instantaneous confidence level of the user's location as well as the newly acquired data correspond to the previous scan data. A visual representation of the degree can be determined and displayed. Finally, once it is determined that the location and orientation of the SLAM has been determined sufficiently for the previous scan data, the scan can continue.

  As explained above, this rewind function is partially enabled by storing the data. It is possible to estimate how many points are carried in every second and then estimate how much to "rewind". The unit informs the user where he was x seconds ago and allows the user to move to that location and take some scans to make sure he is in the right place be able to. For example, the approximate location to go can be communicated to the user (or indicate where the user wants to resume). If the user is close enough, the unit can calculate where the user is and tell the user that he is close enough.

  In another exemplary embodiment, the unit can operate in a transition between spaces. For example, when a user quickly passes through a narrow doorway, there may not be enough data and time to determine the user's location in the new space. In particular, in this example, the border of the door frame may prevent the rider from imaging an amount of the environment behind the door that is sufficient to determine the user's location before proceeding therethrough. One option is to detect this drop in the reliability metric and modify the behavior of the operator to slow down as he approaches a narrow passage, eg, flash a visual indicator, change the color of the screen And the like, to signal the operator.

  Referring to FIG. 21, an exemplary, non-limiting embodiment of an overview of the SLAM unit 2100 is shown. SLAM unit 2100 may include a timing server for generating a plurality of signals derived from a pulse per second (PPS) signal of IMU 2106. The generated signal can be used to synchronize data collected from various sensors in the unit. A microcontroller 2102 can be used to generate these signals and communicate with the CPU 2104. The orthogonal decoder 2108 can be built either within this microcontroller or on an external IC.

  In some exemplary embodiments, IMU 2206 provides a rising edge PPS signal that is used to generate timing pulses for other parts of the system. The camera receives one rising edge signal, described above, generated from the IMU PPS signal and two GPIO1 (one continuous frame) and GPIO2 (two continuous frames) illustrated with reference to FIG. And three falling edge signals.

  As illustrated, each camera receives a trigger signal synchronized with the IMU PPS having a high frame rate of about 30 Hz or 40 Hz and a high resolution of about 0.5-5 Hz.

  Each IMS PPS can zero the internal counter of the microcontroller 2202. The rider's sync output can trigger the following events:

  Reading the current encoder value through an orthogonal decoder.

  ・ Read the current counter value.

  The encoder values and counter values can be stored together and sent to the CPU. This step can occur every 40 Hz defined by the lidar sync output illustrated with reference to FIG.

  Another time synchronization technique may include IMU-based pulse-per-second synchronization that facilitates synchronizing the sensor with the computer processor. An exemplary, non-limiting embodiment of this type of synchronization is shown with reference to FIG.

  IMU 2400 can be configured to send a pulse per second (PPS) signal 2406 to rider 2402. Each time a PPS is sent, the computer 2404 is notified by recognizing a flag in the IMU data stream. Next, the computer 2404 follows and sends a time character string to the rider 2402. Rider 2402 encodes a time stamp in the rider data stream based on the received time string in synchronization with PPS 2406.

  Upon receiving the first PPS 2406, the computer 2404 records its own system time. Starting from the second PPS, the computer 2404 increases the recording time by one second, sends the resulting time string to the rider 2402, and subsequently corrects its own system time to track the PPS 2506.

  In this time synchronization scheme, the IMU 2400 functions as a time server, but the initial time is obtained from the computer system time. The data stream of the IMU 2400 is associated with a time stamp based on the clock of the IMU itself and is initialized with the computer system time when the first PPS 2406 is sent. Thus, IMU 2400, rider 2402, and computer 2404 are all time synchronized. In embodiments, the rider 2402 may be a Velodyne rider.

  According to an exemplary and non-limiting embodiment, the unit includes a COM express board for scanning and a one-button interface.

  According to exemplary and non-limiting embodiments, the processing IMU, the visual data sensor, and the laser data sensor can be combined. The unit can operate for long periods of time in a dark or unstructured environment. In some embodiments, four CPU threads, each running at 5 Hz with Velodyne data for scan matching, may be used. As mentioned above, the movement of the unit may be fast, the unit may be localized relative to the previous map, and the map may be expanded using the localization. The unit exhibits a relatively high CPU usage in the mapping mode and a relatively low CPU usage in the localization mode and is therefore suitable for long term.

  According to exemplary and non-limiting embodiments, the methods and systems described herein allow for coordination between mapping from the ground and mapping from the air according to respective characteristics. Terrestrial-based mappings are not always susceptible to space or time constraints. Generally, mapping devices carried by ground vehicles are suitable for large-scale mapping and can move at high speed. On the other hand, a complex area can be mapped in a hand-held deployment. However, ground-based mapping is limited by the altitude of the sensor, which makes it difficult to implement a top-down configuration. As illustrated in FIG. 25, a ground-based experiment produces a detailed map of the building perimeter, whereas the roof must be mapped from the air. When small aircraft are used, aerial mapping is limited due to the short life of the battery. It is also necessary for the spacecraft to have sufficient visibility to operate safely.

  The collaborative mapping described herein can utilize laser scanners and cameras and low quality IMUs to process data with multi-layer optimization. The resulting motion estimate can be fast (~ 200 Hz) with low drift (typically <0.1% of the travel distance).

  Utilizing the high precision processing pipeline described herein, a map generated from the ground and a map generated from the air can be fused in real time or near real time. This fusion is achieved in part by the localization of one output from the ground derived map to the output from the aerial derived map.

  The method described herein accomplishes collaborative mapping and further reduces the complexity of airborne deployment. A flight path is defined using a ground-based map, and the vehicle performs an autonomous mission. In some embodiments, the vehicle can autonomously perform difficult flight tasks.

  Referring to FIG. 26, there is shown an exemplary, non-limiting embodiment of a sensor / computer pack that can be utilized to enable collaborative mapping. Processing software is not necessarily limited to a particular sensor configuration. Referring to FIG. 26A, a sensor pack 2601 includes a laser scanner 2603 generating 0.3 million points / second, a camera 2605 having a resolution of 640 × 360 pixels and a frame rate of 50 Hz, and a low quality of 200 Hz. And the IMU 2607. An on-board computer processes the data from the sensors in real time for self-motion estimation and mapping. FIG. 26B and FIG. 26C illustrate the sensor field of view. The overlap is shared by the laser and the camera, with which the processing software associates depth information from the laser with the image features, as described more fully below.

  According to exemplary and non-limiting embodiments, the software processes data from distance sensors, such as laser scanners, cameras, and inertial sensors. Instead of combining data from all sensors into a large, full-scale problem, the methods and systems described herein parse the problem as multiple small problems, and map these problems in a coarse-to-fine fashion. Solve in order. FIG. 27 illustrates a block diagram of a software system. In such a system, the forward module performs light processing, ensuring that high frequency motion estimation is robust to extreme motion. The modules behind are responsible for sufficient processing and run at low frequency to ensure the accuracy of the resulting motion estimation and map.

  The software starts with IMU data processing 2701. This module runs at the IMU frequency to predict motion based on IMU mechanization. The result is further processed by the visual-inertial coupling module 2703. Module 2703 tracks distinct image features through the image sequence and solves for motion in the optimization problem. In this case, the laser distance measurements are registered on the depth map, whereby the depth information is associated with the tracked image feature. Since the sensor pack contains only a single camera, depth from the laser helps to resolve scale ambiguity during motion estimation. The estimated motion is used to locally align the laser scan. In a third module 2705, these scans are aligned to further refine the motion estimation. The aligned scan is registered on the map while the scan is aligned with the map. To speed up processing, scan matching utilizes multiple CPU threads in parallel. The map is stored in voxels to speed up querying of points during scan matching. Since motion is estimated at various frequencies, a fourth module 2707 in the system obtains these motion estimates for integration. The output retains both high precision and low latency useful for vehicle control.

  The modularized system further ensures robustness with respect to sensor degradation by selecting "sound" sensor modes when forming the final solution. For example, when the camera is in a low-light or uneven environment, e.g., facing a clean white wall, or the laser is in a symmetrical or extruded environment, such as a long, straight corridor. Sometimes, processing generally fails to generate a valid motion estimate. The system can automatically determine the degraded subspace in the problem state space. When degradation occurs, the system only partially solves the problem in a well-conditioned subspace of each module. As a result, the "healthy" parts are combined to produce a final reasonable motion estimate.

  When a map is available, the method described above can be extended to utilize this map for localization. This extension is accomplished using a scan matching method. The method extracts two types of geometric features, particularly edges and points on a plane, based on the curvature in the local scan. The feature points are aligned with the map. Edge points are aligned to edge line segments and planar points are aligned to local planar patches. Edge segments and local plane patches are determined on the map by examining the eigenvalues and eigenvectors associated with the local point set. Maps are stored in voxels to speed up processing. Localization solves an optimization problem that minimizes the total distance between a feature point and its corresponding point. The optimization usually converges in a few iterations, due to the use of precision odometry estimation to provide an initial guess for localization.

  Localization does not always process individual scans, but stacks some scans for batch processing. Due to the high accuracy odometry estimation, the scan is closely aligned in the local coordinate frame, and the drift can be ignored for a short period of time (eg a few seconds). The comparison is illustrated with reference to FIG. 28 (8.4), where FIG. 28 (a) is a single scan matched in the previous section (scan matching was performed at 5 Hz). , FIG. 28 (b) shows a scan stacked for 2 seconds and aligned during stereotaxy (scan alignment was performed at 0.5 Hz). It can be seen that the stacked scan contains significantly more structural detail and contributes to localization accuracy and robustness to environmental changes. In addition, low frequency execution keeps CPU utilization for on-board processing to a minimum (localization consumes only about 10% of CPU threads). Stacked scans contain significantly more structural detail and contribute to localization accuracy and robustness to environmental changes. Roughly, about 25% of the environment can change or be dynamic, and the system will continue to work well.

  The localization is compared to a particle filter based implementation. Odometry estimation provides a motion model to the particle filter. The particle filter uses 50 particles. At each update stage, the particles are resampled based on low variance resampling. The comparison results are shown in FIG. 29 and Table 8.1. In this case, the error is defined as the absolute distance from the localized scan to the map. During evaluation, the methods and systems described herein select some planes and use the distance from a point in the localized scan to the corresponding plane patch on the map. FIG. 29 illustrates an exemplary, non-limiting embodiment of the error distribution. When the particle filter is run at the same frequency (0.5 Hz) as the method described, the resulting error is five times greater. In contrast, in Table 8.1, the CPU processing time is more than doubled. In another test, running the particle filter at 5 Hz helps to reduce errors to slightly greater than the methods of the present disclosure. However, the corresponding CPU processing time is increased by a factor of 22. These results imply that particle filter based methods do not always take full advantage of high accuracy odometry estimation.

(Table 8)
Table 8.1 Comparison of CPU processing time in localization. When running a particle filter at 5 Hz, high density data is processed at 25% of real-time speed due to high CPU requirements.

  Referring to FIG. 30, an exemplary, non-limiting embodiment of a sensor survey when the sensor pack is held horizontally in the garage is shown. FIG. 30A shows the constructed map and the sensor trajectory. FIG. 30B shows a single scan. In this scenario, the scan contains sufficient structural information. When bypassing the camera processing module, the system generates the same trajectory as the full pipeline. On the other hand, the methods and systems described herein perform another test with a downward tilt sensor pack perpendicular to the ground. The result is shown in FIG. In this scenario, the structural information in the scan is quite sparse, as shown in FIG. The process fails without the use of a camera and succeeds using a full pipeline. The result is important for high altitude flights where a tilt of the sensor pack is required.

  Referring to FIG. 32, there is shown an exemplary, non-limiting embodiment where the sensor pack is held by an operator passing through a circle at a speed of 1-2 m / s, for a total travel distance of 410 m. FIG. 32 (a) shows the resulting map and sensor trajectory using the horizontal orientation sensor configuration. The sensors are started and stopped at the same position. This test produces a drift of 0.18 m through the path, resulting in a 0.04% relative position error compared to the distance traveled. The operator then repeats this path using two sensor packs held at 45 ° and 90 ° angles, respectively. The resulting sensor trajectory is shown in FIG. Clearly, the slope introduces a larger drift, with a relative position error of 0.6% at 45 ° (dashed blue curve) and 1.4% at 90 ° (red dot-dash curve). . Finally, the drift is offset by the localization on the map of FIG. 32 (a), and both configurations result in a solid black curve.

  An exemplary, non-limiting embodiment of the drone platform 3301 is shown in FIG. The aircraft weighs about 6.8 kg (including batteries) and can carry a maximum payload of 4.2 kg. The sensor / computer pack is mounted on the bottom of the aircraft and weighs 1.7 kg. The remote controller is shown at the lower right of this figure. The remote controller is operated by the safety pilot to override autonomy as needed during the autonomy mission. Note that the aircraft is constructed with a GPS receiver (on top of the aircraft). GPS data is not always used in mapping or autonomous performance.

  In the first collaborative mapping experiment, the operator walks around the building while holding the sensor pack. The result is shown in FIG. In FIG. 25 (a), ground-based mapping is performed at 1-2 m / s over a 914 m journey to cover the perimeter of the building in detail. As expected, the roof of the building is blank on this map. Next, the drone is remotely controlled to fly over the building. In FIG. 25 (b), this flight is performed at 2-3 m / s, and the traveling distance is 269 m. This process uses the localization of the map shown in FIG. In this manner, the aerial map is fused with the ground-based map (white points). After the ground-based map has been constructed, the take-off location of the drone is determined on the map. The sensor starting attitude for aerial mapping is known and localization starts there. FIG. 34 provides air and ground based sensor trajectories in top and side views.

  Further, the methods and systems described herein implement autonomous flight to achieve aerial mapping. Referring to FIG. 35, a ground-based map is first constructed by hand-held mapping over a 672 m journey at 1-2 m / s to the flight zones. This map and the sensor trajectory are shown in FIG. Subsequently, path points are defined based on this map, and the drone follows these path points to perform aerial mapping. As shown in FIG. 35 (b), the curve is a flight path, and a large point on the curve is a path point, and the points form an aerial map. In this experiment, the drone takes off from the hangar on the left side of the figure, flies across the site, passes through another hangar on the right side, and then returns to the first hangar to land. The speed is 4 m / s when crossing the scene and 2 m / s when passing through the hangar. FIG. 35 (c) and FIG. 35 (d) are two images taken by the onboard camera when the drone is flying towards the hangar to the right and when trying to enter this hangar. FIG. 35E shows the estimated speed during the mission.

  Finally, the methods and systems described herein perform another experiment over a longer distance. As shown in FIG. 36, the ground-based mapping includes an off-road vehicle driven at 10 m / s from the left to the right over a 1463 m journey. Using a ground-based map and path points, the autonomous flight traverses the scene. After takeoff, the drone climbs at 15m / s to a height of 20m above the ground. After that, the drone descends 2m above the ground and passes through the trees at 10m / s. The flight path is 1118 m long as shown by the curve 3601 in FIG. The two images are when the drone is flying high above the trees (see FIG. 36 (c)) and when it is flying low below the trees (see FIG. 36 (d)). Will be taken.

  Exemplary and non-limiting embodiments allow global positioning data, such as from GPS, to be incorporated into a processing pipeline. Global positioning data can offset self-motion estimation drift over long trip distances and can be useful for aligning maps within a global coordinate frame.

  In some embodiments, the GPS data can be recorded simultaneously with the mapping activity. As the system moves, there is some level of drift that causes the error to grow with distance. In general, you may only experience a 0.2% drift, but when traveling 1000 meters nonetheless, the drift is 2 meters for every 1000 meters of travel. At 10 km, the drift grows to 20 meters and so on. Without closing the loop (returning to the beginning of the route in the conventional sense), this error cannot be corrected. By providing the external information in the form of GPS coordinates, the system can understand where it is and correct the current position estimate. Typically, this correction is made in a post-processing effort, but the system of the present invention can perform such correction in real time or near real time.

  Thus, the use of GPS provides a way to allow the loop to be closed. Of course, GPS also has some amount of error, but is usually consistent within a given area, and many GPS systems today provide greater than 30 cm accuracy at locations X and Y on the surface. Can be. Other more expensive and sophisticated systems can provide cm-level positioning.

  The use of this GPS provides the following important functions. 1. Point cloud localization on earth. 2. The course was corrected so that the map was more accurate because its location was understood, and that any data acquired at that location could be associated with a series of similarly collected GPS points. The ability to align and "fix" maps using information. 3. A function that allows the system to act as an IMU when GPS is lost.

  The exemplary and non-limiting embodiment allows the use of dynamic vision sensors to further improve the estimated robustness for extreme movements. Dynamic vision sensors report data only on pixels with lighting changes, producing both high speed and low latency.

  This high speed (generally defined as higher than about 10 Hz) can quickly provide fast information to the self-motion and estimation system, thereby improving the localization and subsequently the value for the mapping. . When the system can capture more data with less delay, the system becomes more accurate and more robust. Features that are identified and tracked by the dynamic vision sensor allow for more accurate estimations because more features and faster updates allow for more accurate tracking and motion estimation.

  A direct method can be used to achieve image matching with a dynamic vision sensor for self-motion estimation. In particular, the direct method matches successive images for feature tracking from image to image. On the other hand, the feature tracking method disclosed herein is superior to the direct method.

  The illustrative and non-limiting embodiments allow for implementing parallel processing that runs on a general purpose GPU or FPGA, thus allowing for data processing at higher volumes and higher frequencies .

  Using parallel processing techniques, some features can be tracked simultaneously, or data in a predetermined area or direction can be partitioned into multiple smaller problems. A parallel architecture may take the form of multiple cores, threads, processors, or even special forms such as a graphics processing unit (GPU). By using the divide-and-conquer method, the overall processing can be significantly speeded up, since each node in the parallel architecture is simultaneously dedicated to that node's part of the problem. Generally, such speedups are not linear. In other words, by dividing the problem into 16 parts and processing, each part does not necessarily speed up processing individually 16 times. There is overhead in performing splits, communicating, and then reorganizing the results.

  In contrast, when using linear processing, each subproblem or computation is processed once, data is moved into and out of memory, CPU, and then back to memory or storage. Although this process is slow, the pipeline architecture, when properly configured with a systolic array or the like, can improve the situation as long as the pipeline is kept filled.

  Exemplary and non-limiting embodiments can introduce a loop closure to remove the drift of the self-motion estimation by global smoothing.

  For example, when returning to the starting point, since it is already there, it is understood that the position estimation can be corrected. The starting point is the origin and there should be accumulated errors that become apparent at the end of the movement. The error value can be determined by taking the difference between the origin (0,0,0) and the position (for example, (1,2,3)) considered to be at the time of returning to the starting end. However, this value is possibly evenly accumulated over travel.

  For example, if one moves 100 meters back to the starting point and finds that xyz is off by one meter, then the problem arises of determining where to apply this error during the entire distance. One approach is to spread this error over the entire distance, where it stays on the path and more importantly corrects the stroke by 1 cm at each meter increment in order to return to the starting point without error. Can be. This correction is wide area smoothing.

  Another approach is to use any other error measure during the move and apply the correction only where this error measure is high. For example, the covariance matrix provides a good metric for map quality and can be used to spread this error a priori over the course of all movements. In other words, using locations with low quality matching that appear in the covariance matrix can be used to distribute the errors unequally throughout the movement according to the ratio of the errors over the movement. This variance will act to correct for errors in the detail area where the low quality values are associated with a particular location.

  The following clauses provide additional statements regarding the embodiments disclosed herein.

  Article 1. Obtaining a rider point cloud including a plurality of points each having at least geospatial coordinates and a segment as an attribute, and a reliability indicating a calculation accuracy of a plurality of points each having the same segment as an attribute; A method comprising: assigning a level; and adjusting a geospatial coordinate of each of at least a portion of the plurality of points to which the same segment has been attributed based at least in part on a confidence level.

  Article 2. Clause 1 wherein the confidence level is the confidence level of the loop closure.

  Article 3. The method of Clause 1, wherein adjusting the geospatial coordinates is responsive to the determined loop closure error.

  Article 4. Clause 1. The method of Clause 1, wherein the segment for which the geospatial coordinates are adjusted has a lower confidence level than at least one other segment.

  Section 5. Starting a step of using a SLAM to obtain a rider point cloud including a plurality of points each having a start location and at least geospatial coordinates and a segment as attributes, and acquiring the rider point cloud. A method comprising: moving a loop; and determining a scan end point when the SLAM approaches a start location.

  Article 6. 7. The method of clause 6, wherein determining the scan end point comprises receiving from the SLAM user an indication that the loop has been moved.

  Article 7. Clause 6. The method of Clause 6, wherein determining the scan end point is based on a point in the segment indicating proximity to the start location.

  Article 8. Clause 6. The method of Clause 6, wherein geospatial coordinates that are proximal to the start location are assigned as attributes to points in the segment other than the segment that includes the start location.

  Article 9. Obtaining a rider point cloud including a plurality of points each having at least geospatial coordinates and a time stamp as attributes; and a plurality of points each having at least one of geospatial coordinates and a time stamp as attributes. Acquiring color image data including the image of the image, and a geospatial coordinate having a time stamp that is temporally close to the time stamp of each point to be colored, and further being closest to the geospatial coordinates of the plurality of points to be colored. Coloring at least a portion of the plurality of points using color information derived from the image having

  Article 10. Clause 9. The method of clause 9, wherein the coloring is performed in real time or near real time.

  Article 11. Deriving a motion estimate for the SLAM system using an IMU forming part of the SLAM system, and refining the motion estimate by a visual-inertial odometry optimization process to generate a refined estimate; Refining the refined estimate by minimizing at least one residual square error between at least one feature in the current scan and at least one previously scanned feature by a laser odometry optimization process And a step.

  Article 12. 12. The method of clause 11, wherein deriving the motion estimate comprises receiving the IMU update at a frequency of about 200 Hz.

  Article 13. Clause 12. The method of Clause 12, wherein the frequency of the IMU update is between 190 Hz and 210 Hz.

  Article 14. 12. The method of clause 11, wherein refining the motion estimation comprises refining the motion estimation at a rate equal to a frame rate of a camera forming part of the SLAM system.

  Article 15. Clause 14 wherein the frame rate is between 30Hz and 40Hz.

  Article 16. Clause 11. The method of Clause 11, wherein the laser odometry optimization process is performed at a scan frame rate at which the lidar rotation mechanism forming part of the SLAM scans a complete data hemisphere.

  Article 17. Obtaining a plurality of depth tracking visual features in a plurality of camera frames using a camera forming part of the SLAM system; and obtaining the plurality of visual features from a rider forming part of the SLAM. A method comprising associating with a lidar derived point cloud and triangulating a depth of at least one visual feature between at least two camera frames.

  Article 18. Clause 17. The method of Clause 17, wherein the associating and triangulation steps are performed on a processor using a parallel computer.

  Article 19. A SLAM device including a microcontroller, an inertial measurement unit (IMU) adapted to generate a plurality of timing signals, and a timing server adapted to generate a plurality of synchronization signals derived from the plurality of timing signals. Clause wherein the synchronization signal is operative to synchronize at least two sensors forming part of the SLAM device.

  Article 20. Clause 20. The SLAM device of Clause 20, wherein the at least two sensors are selected from the group consisting of a rider, a camera, and an IMU.

  Article 21. Acquiring a rider point cloud including a plurality of points each having at least geospatial coordinates and a time stamp as attributes, and a plurality of images each having at least geospatial coordinates and a time stamp as attributes. Obtaining color image data including at least one of a time stamp temporally close to a time stamp of each point to be colored and a geospatial coordinate close to a geographical coordinate of each point to be colored. A method comprising: coloring at least a portion of a plurality of points using color information derived from an image; and displaying a colored portion of the plurality of points.

  Article 22. 22. The method of clause 21, further comprising displaying the output from the camera as an overlay on the displayed points.

  Article 23. Obtaining a rider point cloud including a plurality of points each having at least geospatial coordinates and a time stamp as attributes, and coloring at least a portion of the plurality of points using color information. , Wherein each of the plurality of points is colored using a color corresponding to a parameter of the acquired lidar point cloud selected from the group consisting of an intensity parameter, a density parameter, a time parameter, and a geospatial location parameter.

  Article 24. Acquiring, using SLAM, a rider point cloud including a plurality of near points derived from the corresponding near environment and a plurality of far points derived from the corresponding far environment, using the SLAM. Scanning through one or more spaces between one or more elements located in the near environment and utilizing the plurality of far points as the SLAM moves from the near environment to the far environment. The method by which is determined.

  Article 25. Receiving, in a SLAM system, feedback including a plurality of feedback terms from at least one of a camera and a laser; and modeling a plurality of biases each associated with the plurality of feedback terms, the plurality of biases being included. There is a method of forming a sliding window for bias.

  Article 26. Clause 25. The method of Clause 25, wherein each of the plurality of feedback terms comprises an estimated piecewise motion of the SLAM system.

  Article 27. Clause 26. The method of Clause 26, wherein each of the plurality of bias terms is modeled to be constant during the piecewise exercise.

  Article 28. Clause 25. The method of Clause 25, wherein the sliding window includes a bias between 200 and 1000.

  Article 29. Clause 28. The method of Clause 28 wherein the sliding window includes about 400 biases.

  Article 30. Clause 25. The method of Clause 25, wherein the sliding window serves as a parameter for determining a rate of update of the plurality of biases.

  Article 31. Clause 25. The method of Clause 25, wherein the sliding window is adapted to allow dynamic reconfiguration of the SLAM system.

  Article 32. Clause 25. The method of Clause 25, further comprising performing an IMU bias correction for the plurality of biases.

  Article 33. 33. The method of clause 32, wherein performing IMU bias correction includes utilizing data from at least one of a laser and a camera.

  Article 34. Clause 33. The method of Clause 33 wherein the laser and camera form part of a SLAM system.

  Article 35. Clause 33. The method of Clause 33 wherein each of the laser and the camera include estimated piecewise motion over time.

  Article 36. Clause 25. The method of Clause 25 wherein the sliding window is an array formed with a predetermined number of biases, wherein the biases are added and deleted in a first-in / first-out manner.

  Article 37. The camera and the IMU receiving visual data from the camera forming part of the SLAM system and receiving inertial data from the IMU, and estimating the piecewise motion of the SLAM system using the visual data and the inertial data as constraints. A method comprising the steps of: associating depth information with one or more visual features derived from visual data.

  Article 38. Clause 37 wherein the depth information is obtained from the laser data.

  Article 39. Clause 38. The method of Clause 38 wherein depth information can be calculated from triangulation using estimated piecewise motion when laser data is not available.

  Article 40. Clause 37. The method of Clause 37 used to construct a point cloud with depth information aligned.

  Article 41. 41. The method of clause 40, further comprising calculating one or more eigenvectors for the point cloud and using the one or more eigenvectors to specify degradation of the point cloud.

  Article 42. Clause 41. The method of Clause 41 wherein a degradation in a direction of the state space indicated by at least one of the eigenvectors is used to discard a solution in that direction.

  Article 43. Determining the relative pose between a camera having a camera coordinate system and a laser having a laser coordinate system both forming part of a SLAM system by utilizing a single coordinate system for the camera and the laser Determining the relative attitude between the laser and an IMU that forms part of a SLAM system and has an IMU coordinate system.

  Article 44. Clause 43. The method of Clause 43 wherein utilizing a single coordinate system comprises projecting a plurality of laser points into a camera coordinate system during preprocessing.

  Article 45. Clause 43. The method of Clause 43 wherein the IMU coordinate system is substantially parallel to the camera coordinate system.

  Article 46. Clause 45. The method of Clause 45 further comprising the step of rotationally correcting IMU data from the IMU upon its acquisition.

  Article 47. Clause 43. The method of Clause 43 wherein the camera coordinate system has the x-axis pointing left, the y-axis pointing up, the z-axis pointing forward, and the origin being at the camera optical center coincident with the main axis of the camera.

  Article 48. Clause 43. The system of Clause 43, wherein the IMU coordinate system is originated from an IMU measurement center in which the x-axis, y-axis, and z-axis are parallel to the same direction and point in the same direction.

  Article 49. Clause 43. The system of Clause 43, wherein the world coordinate system is a coordinate system that matches the initial attitude of the SLAM system.

  Clause 50.1 Establishing a motion model of the mobile mapping system using one or more pose constraints, and a mobile mapping system including one or more landmark locations using one or more camera constraints. Establishing a landmark measurement model for each of the above, and solving for each of the motion model and the landmark measurements.

  Article 51. Clause 50. The method of Clause 50 wherein the step of solving for each model includes the step of utilizing Newton's gradient descent.

  Article 52. Clause 51. The method of Clause 51 wherein Newton's gradient descent is applied to a robust fitting framework for removal of off-going features.

  Article 53. Clause 50. The method of Clause 50, wherein the solution is performed without optimizing the plurality of landmark locations.

  Article 54. Receiving an odometry estimate including a plurality of key points from a visual-inertial odometry module of the mobile mapping system; receiving a plurality of IMU measurements from an IMU forming part of the mobile mapping system; Aligning a plurality of laser points collected from the laser forming the part based at least in part on the IMU measurements.

  Article 55. Clause 54. The method of Clause 54 wherein the step of aligning comprises utilizing the IMU measurements to interpolate between the key points.

  Article 56. Clause 55. The method of Clause 55, wherein the step of interpolating comprises selecting one or more geometric features to be used for tracking from the point cloud formed by the key points.

  Article 57. Clause 56. The method of Clause 56 wherein the bad points in the point cloud are not selected based on the geometry.

  Article 58. Clause 56. The method of Clause 56 wherein the bad points in the point cloud are not selected based on a relationship between one or more points and a surface in the point cloud.

  Article 59. Storing map information including a point cloud in a plurality of first level voxels each occupying a first volume of the same size; and storing a plurality of map information each occupying a second volume of the same size. Storing map information in a second level voxel, wherein the first volume is greater than the second volume, and storing a map information in the second level voxel; Extracting map information from the voxels, performing scan matching on the extracted second-level voxels, and using the first-level voxels for map information composed of the first-level voxels. And maintaining.

  Article 60. Clause 59. The method of clause 59, wherein each first level voxel is mapped to a plurality of second level voxels.

  Article 61. Clause 59. The method of Clause 59 wherein the second level voxels are stored in a 3D KD tree.

  Article 62. Clause 59. The method of Clause 59 further comprising downsizing the point cloud to maintain a substantially constant point density.

  Article 63. A method comprising providing a ground origin cloud map, generating an aerial origin cloud map, and fusing the ground origin cloud map and the aerial origin cloud map in real time or near real time.

  Article 64. Clause 63. The method of Clause 63 wherein the aerial origin cloud map is generated utilizing a drone including a mobile mapping system.

  Article 65. Clause 63. The method of clause 63 wherein the drone includes a laser scanner, a camera, and a low quality IMU.

  Article 66. Clause 65. The method of Clause 65 wherein data from the laser scanner, camera, and IMU is processed by a multi-layer optimization process.

  Article 67. Clause 63. The method of Clause 63 wherein the step of fusing further comprises the step of localizing at least one output from the ground derived map to an aerial origin cloud map.

  Article 68. The method of Clause 64, wherein generating the aerial origin cloud map comprises defining a drone flight path and autonomously flying the drone according to the drone flight path.

  While only a few embodiments of the present disclosure have been shown and described, many changes and modifications may be made to these embodiments without departing from the spirit and scope of the present disclosure, which is set forth in the following claims. It will be apparent to those skilled in the art that modifications can be made. All patent applications and patents, both foreign and domestic, and all other publications cited herein are hereby incorporated by reference in their entirety to the fullest extent permitted by law. .

  The methods and systems described herein may be partially or wholly deployed by machines that execute computer software, program code, and / or instructions on a processor. The present disclosure may be applied to one or more of the machines as a method on a machine, as part of a machine or as a system or device associated with a machine, or embodied in a computer-readable medium. And can be implemented as a computer program product executed on a computer. In embodiments, the processor may be part of a server, cloud server, client, network infrastructure, mobile computer platform, fixed computer platform, or other computer platform. A processor may be any type of computing or processing device capable of executing program instructions, code, binary instructions, and the like. The processor may be a signal processor, digital processor, embedded processor, microcontroller, or coprocessor that facilitates, directly or indirectly, the execution of internally stored program code or instructions. And communication co-processors) and the like. In addition, a processor may allow for execution of multiple programs, threads, and code. Multiple threads may execute concurrently to improve processor performance or to facilitate concurrent operation of applications. Depending on the implementation, the methods, program codes, program instructions, and the like described herein can be implemented in one or more threads. Threads can spawn other threads with which they can associate an allocation priority, and the processor executes these threads based on priority or any other order based on instructions given in the program code. can do. The processor or any machine utilizing the same may include non-transitory memory for storing the methods, codes, instructions, and programs described herein and elsewhere. The processor can access the non-transitory storage media through the interface, which can store the methods, codes, and instructions described herein and elsewhere. The storage media associated with the processor for storing the methods, programs, code, program instructions, or other types of instructions that can be executed by the computing device or the processing device include CD-ROM, DVD, memory, hard disk, It may include, but is not limited to, one or more of a flash device, RAM, ROM, cache, and the like.

  A processor can include one or more cores that can increase the speed and performance of a multiprocessor. In embodiments, the processing may be a dual-core processor, a quad-core processor, and other chip-level multiprocessors that combine two or more independent cores (called dies).

  The methods and systems described herein may be partially or wholly implemented by servers, clients, firewalls, gateways, hubs, routers, or other such machines that execute computer software on computer and / or network connection hardware. Can be deployed dynamically. Software programs can be associated with servers that can include file servers, print servers, domain servers, Internet servers, intranet servers, cloud servers, and other variants such as secondary servers, host servers, distributed servers, and the like. it can. Servers can access memory, processors, computer readable media, storage media, ports (physical and virtual), communication devices, and other servers, clients, machines, and devices through wired or wireless media. One or more of an interface or the like may be included. The methods, programs, or codes described herein and anywhere else may be executed by a server. In addition, other devices required to perform the methods described in this application can be considered as part of the infrastructure associated with the server.

  The server may provide an interface to clients, other servers, printers, database servers, print servers, file servers, communication servers, distributed servers, and other devices, including without limitation social networks and the like. In addition, this coupling and / or connection can facilitate remote execution of programs across a network. The network connection of some or all of these devices may facilitate parallel processing of programs or methods at one or more locations without departing from the scope of the present disclosure. In addition, any of the devices attached to the server through the interface may include at least one storage medium capable of storing methods, programs, codes, and / or instructions. A central repository may provide program instructions that will be executed on different devices. In this implementation, the remote repository can serve as a storage medium for the program code, instructions, and programs.

  Software programs can be associated with clients that can include file clients, print clients, domain clients, Internet clients, intranet clients, and other variants such as secondary clients, host clients, distributed clients, and the like. Clients can access memory, processors, computer readable media, storage media, ports (physical and virtual), communication devices, and other clients, servers, machines, and devices through wired or wireless media. One or more of an interface or the like may be included. The methods, programs, or codes described herein and anywhere else may be executed by a client. In addition, other devices required to perform the methods described in this application can be considered part of the infrastructure associated with the client.

  A client can 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. In addition, this coupling and / or connection can facilitate remote execution of programs across a network. The network connection of some or all of these devices may facilitate parallel processing of programs or methods at one or more locations without departing from the scope of the present disclosure. In addition, any of the devices attached to the client through the interface can include at least one storage medium that can store the methods, programs, applications, codes, and / or instructions. A central repository may provide program instructions that will be executed on different devices. In this implementation, the remote repository can serve as a storage medium for the program code, instructions, and programs.

  The methods and systems described herein may be partially or wholly deployed by a network infrastructure. The network infrastructure is a computing device, server, router, hub, firewall, client, personal computer, communication device, routing device, other active or passive device, module, and / or component known in the art. Can be included. Computing devices and / or non-computer devices associated with a network infrastructure, apart from other components, can include storage media such as flash memory, buffers, stacks, RAM, ROM, and the like. The processes, methods, program codes, instructions described herein and elsewhere may be performed by one or more of the network infrastructure elements. The methods and systems described herein may characterize software as a service (SaaS), platform as a service (PaaS), and / or infrastructure as a service (IaaS). It can be adapted for use with any type of private, community, or hybrid cloud computer network or environment, including those involved.

  The methods, program codes, and instructions described herein and elsewhere may be implemented on a cellular network having a source-controlled contact media content item multiple cells. A cellular network can be either a frequency division multiple access (FDMA) network or a code division multiple access (CDMA) network. A cellular network can include mobile devices, cell sites, base stations, repeaters, antennas, towers, and the like. The cell network may be of the GSM, GPRS, 3G, EVDO, mesh, or other network type.

  The methods, program codes, and instructions described herein and anywhere else can be implemented on or by a mobile device. Mobile devices can include navigation devices, cell phones, mobile phones, mobile personal digital assistants, laptops, palmtops, netbooks, pagers, e-book readers, music players, and the like. These devices, apart from other components, may include storage media such as flash memory, buffers, RAM, ROM, and one or more computing devices. A computing device associated with the mobile device can be operable to execute program codes, methods, and instructions stored thereon. Alternatively, the mobile device can be configured to execute instructions in cooperation with other devices. The mobile device can communicate with a base station that is configured to interface with the server and execute the program code. Mobile devices can communicate over peer-to-peer networks, mesh networks, or other communication networks. The program code can be stored on a storage medium associated with the server and can be executed by a computing device embedded inside the server. A base station can include a computing device and a storage device. The storage device can store program codes and instructions that are executed by a computing device associated with the base station.

  Computer software, program code, and / or instructions may include computer components, devices, and storage media for holding digital data used by the computer for some time interval; semiconductor storage, known as random access memory (RAM); Mass storage for more permanent storage, such as magnetic storage in the form of hard disks, tapes, drums, cards, and other types such as optical disks, and processor registers, cache memory, volatile memory, Non-volatile memory, optical storage such as CD and DVD, flash memory (for example, USB stick or USB key), floppy disk, magnetic tape, paper tape, punch card, stand-alone RAM disk, Zip drive Removable mass storage and removable media such as offline, dynamic memory, static memory, read / write storage, variable storage, read-only storage, random access storage, sequential access storage, location addressable Stored on machine readable media that can include storage, file addressable storage, content addressable storage, network attached storage, storage area networks, barcodes, and other computer memory such as magnetic ink, etc. And / or have access to it.

  The methods and systems described herein can convert physical and / or intangible from one state to another. Further, the methods and systems described herein may also convert data representing physical and / or intangible from one state to another.

  Elements described and depicted herein, including those of flowcharts and block diagrams, imply logical boundaries between the elements. However, according to software or hardware engineering conventions, the depicted elements and their functions may be implemented in a monolithic software structure, a standalone software module, or a program stored internally as a module using external routines, code, and services. It can be implemented through a computer-executable medium on a machine having a sender-controlled contact media content item processor capable of executing instructions, all such implementations being within the scope of the present disclosure. It can be. Examples of such machines are 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, sender controlled contact media content items Artificial intelligence devices, computing devices, network connected devices, servers, routers, and the like may be included, but are not limited thereto. Further, the elements depicted in the flowcharts and block diagrams, or any other logical components, may be implemented on machines capable of executing program instructions. Thus, unless expressly stated or otherwise apparent from the context, the foregoing figures and descriptions show the functional aspects of the disclosed system of the present invention, but may include any of the software for implementing these functional aspects. No particular sequence should be inferred from these descriptions. Similarly, it will be appreciated that the various steps illustrated and described above can be varied, and that the order of the steps can be tailored to the particular application of the technology disclosed herein. All such changes and modifications are intended to be within the scope of the present disclosure. Thus, unless otherwise required by a particular application, unless explicitly stated or otherwise apparent from the context, the depiction and / or description of the steps with respect to the various steps requires a particular order of execution for those steps. That should not be understood.

  The methods and / or processes described above, and the associated steps, may be implemented 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 special-purpose computing device, or a particular computing device or particular aspects or components thereof. The processing may be implemented in one or more microprocessors, microcontrollers, embedded microcontrollers, programmable digital signal processors, or other programmable devices, with internal and / or external memory. The processing may also or alternatively be embodied in an application specific integrated circuit, a programmable gate array, a programmable array logic, or any other device or combination of devices that can be configured to process electronic signals. Can be It will further be appreciated that one or more of the operations can be implemented as computer-executable code that can execute on a machine-readable medium.

  The computer-executable code may be on a structured programming language such as C, an object-oriented programming language such as C ++, or on one of the above devices, as well as on a heterogeneous combination of processors, processor architectures, and different hardware. Any other high or low level that can be stored, compiled, or interpreted to run on a combination of software or on any other machine that can execute program instructions (Assembly language, hardware description language, and database programming language and technology).

  Thus, in one aspect, the above-described methods and combinations thereof may be embodied in computer-executable code that, when executed on one or more computing devices, implements the steps of these methods. In another aspect, the method may be embodied in a system that performs the steps, may be distributed across the devices in some manner, or may dedicate all of the functions to a dedicated stand-alone device or other hardware. It can be integrated in the ware. In another aspect, the means for performing the steps associated with the processes described above can 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.

  Although the present disclosure has been disclosed in connection with the preferred embodiments illustrated and described in detail, various modifications and improvements to these embodiments will be readily apparent to those skilled in the art. Therefore, the spirit and scope of the present disclosure should not be limited by the foregoing examples, but rather should be understood in the broadest sense permitted by law.

  The terms "a" and "an" and "the" and similar designations in the context of describing the disclosure of the present invention, particularly in the context of the claims which follow, are used unless otherwise indicated herein. Or should be construed to cover both the singular and the plural, unless the context clearly indicates otherwise. The terms "comprising," "having a sender-controlled contact media content item," "including," and "containing" are non-limiting terms, unless otherwise specified (i.e., "including But not limited to this). The specification of a range of values herein is intended only to serve as an abbreviation for individually writing each individual value that falls within the range, unless otherwise indicated herein. , Each separate value is incorporated herein as if these values were individually expressed herein. All methods disclosed herein can be performed in any suitable order, unless the context clearly indicates otherwise, unless otherwise indicated herein. The use of any and all examples or illustrative language provided herein (eg, “eg,” etc.) is intended only to clarify the disclosure of the invention and, unless otherwise stated, It places no restrictions on the scope of the present disclosure. No language in the specification should be construed as indicating any non-claimed element as essential to the practice of the disclosure.

  While the foregoing written description will allow those skilled in the art to make and use what is considered to be the best mode at this time, those skilled in the art will recognize that certain embodiments, methods, and examples herein may be modified. , Combinations and equivalents will be understood and appreciated. Accordingly, the disclosure of the present invention should be limited not by the embodiments, methods, and examples described above, but by all embodiments and methods that fall within the scope and spirit of the present disclosure.

  All documents cited herein are hereby incorporated by reference.

2100 SLAM unit 2102 Microcontroller 2104 CPU
2106 IMU
2108 orthogonal decoder

Claims (30)

Receiving data from an IMU device at a first frequency at a first calculation module and calculating a first estimated position of the mobile mapping system based at least in part on the received IMU data;
The first estimated position and visual-inertial data are received at a second calculation module at a second frequency, and a second of the mobile mapping system is based at least in part on the first estimated position and visual-inertial data. Calculating an estimated position of 2;
The second estimated position and the laser scan data are received at a third frequency at a third calculation module, and a third of the mobile mapping system is based at least in part on the second estimated position and the laser scan data. Calculating an estimated position;
A method comprising:   The method of claim 1, wherein each of the first frequency, the second frequency, and the third frequency are different from one another.   The method of claim 2, wherein the frequency is higher than the second frequency, and the second frequency is higher than the third frequency.   The method of claim 1, wherein operation of at least one computing module is bypassed.   The method of claim 4, wherein the estimated positions calculated by each of the remaining non-diverted modules are combined in a linear fashion.   5. The method of claim 4, wherein the estimated positions calculated by each of the remaining non-diverted modules are combined in a non-linear manner.   The method of claim 4, wherein when the first calculation module is bypassed, the second estimated position is calculated from the visual inertial data without reference to the first estimated position. .   The method of claim 4, wherein the third estimated position is calculated from the laser scan data and the first estimated position when the second calculation module is bypassed.   The method of claim 1, further comprising determining a degraded subspace in the problem state space and calculating an estimated position in the degraded subspace.   The method of claim 9, wherein the subspace is a well-conditioned subspace.   The method of claim 1, wherein the estimated position calculated by at least one of the second calculation module or the third calculation module is received as an input by a preceding calculation module.   The method of claim 1, wherein the system operates at a high angular velocity.   The method of claim 12, wherein the high angular velocity includes a rotational speed as high as 360 degrees / second.   The method of claim 1, wherein the system is adapted to operate at a high linear velocity.   The method of claim 12, wherein the high linear velocity includes a velocity as high as 100 kilometers / hour. A mobile mapping system,
A first calculation module adapted to receive data at a first frequency from an IMU device and calculate a first estimated position of a mobile mapping system based at least in part on the received IMU data;
Receiving the first estimated position and visual-inertial data at a second frequency and calculating a second estimated position of a mobile mapping system based at least in part on the first estimated position and visual-inertial data. A second calculation module,
Receiving the second estimated position and the laser scan data at a third frequency, and calculating a third estimated position of a mobile mapping system based at least in part on the second estimated position and the laser scan data. A third calculation module,
A mobile mapping system comprising:   17. The system of claim 16, wherein each of the first frequency, the second frequency, and the third frequency are different from each other.   The system of claim 17, wherein the frequency is higher than the second frequency, and wherein the second frequency is higher than the third frequency.   17. The system of claim 16, wherein operation of at least one computing module is bypassed.   20. The system of claim 19, wherein the estimated positions calculated by each of the remaining non-diverted modules are combined in a linear fashion.   20. The system of claim 19, wherein the estimated positions calculated by each of the remaining non-diverted modules are combined in a non-linear fashion.   The system of claim 19, wherein when the first calculation module is bypassed, the second estimated position is calculated from the visual inertia data without reference to the first estimated position. .   20. The system of claim 19, wherein the third estimated position is calculated from the laser scan data and the first estimated position when the second calculation module is bypassed.   17. The system of claim 16, further comprising determining a degraded subspace in the problem state space and calculating an estimated position in the degraded subspace.   The system of claim 24, wherein the subspace is a well-conditioned subspace.   The system of claim 16, wherein the estimated position calculated by at least one of the second calculation module or the third calculation module is received as an input by a preceding calculation module.   17. The system according to claim 16, adapted to operate at a high angular velocity.   The system of claim 27, wherein the high angular velocity includes a rotational speed as high as 360 degrees / second.   17. The system according to claim 16, adapted to operate at a high linear velocity.   The system of claim 27, wherein the high linear velocity includes a velocity as high as 100 kilometers / hour.

Images