CN113959435A - Vehicle-mounted all-around online SLAM system and method based on multi-camera model - Google Patents
Vehicle-mounted all-around online SLAM system and method based on multi-camera model Download PDFInfo
- Publication number
- CN113959435A CN113959435A CN202111120380.9A CN202111120380A CN113959435A CN 113959435 A CN113959435 A CN 113959435A CN 202111120380 A CN202111120380 A CN 202111120380A CN 113959435 A CN113959435 A CN 113959435A
- Authority
- CN
- China
- Prior art keywords
- vehicle
- camera
- module
- camera model
- frame
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 62
- 238000005457 optimization Methods 0.000 claims abstract description 40
- 230000033001 locomotion Effects 0.000 claims abstract description 32
- 238000001514 detection method Methods 0.000 claims abstract description 25
- 238000004364 calculation method Methods 0.000 claims abstract description 23
- 238000005259 measurement Methods 0.000 claims description 18
- 230000001133 acceleration Effects 0.000 claims description 10
- 230000010354 integration Effects 0.000 claims description 5
- 230000003287 optical effect Effects 0.000 claims description 5
- 238000012545 processing Methods 0.000 description 7
- 230000000007 visual effect Effects 0.000 description 7
- 238000006073 displacement reaction Methods 0.000 description 6
- 238000010586 diagram Methods 0.000 description 4
- 230000006870 function Effects 0.000 description 4
- 238000012986 modification Methods 0.000 description 4
- 230000004048 modification Effects 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 3
- 238000011160 research Methods 0.000 description 3
- 238000012795 verification Methods 0.000 description 3
- 238000004891 communication Methods 0.000 description 2
- 238000000354 decomposition reaction Methods 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 238000007726 management method Methods 0.000 description 2
- 239000011159 matrix material Substances 0.000 description 2
- 230000008447 perception Effects 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 239000013598 vector Substances 0.000 description 2
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 238000011423 initialization method Methods 0.000 description 1
- 238000003780 insertion Methods 0.000 description 1
- 230000037431 insertion Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000010606 normalization Methods 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
- G01C21/1656—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with passive imaging devices, e.g. cameras
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/20—Analysis of motion
- G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/20—Analysis of motion
- G06T7/269—Analysis of motion using gradient-based methods
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10016—Video; Image sequence
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Multimedia (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Theoretical Computer Science (AREA)
- Image Analysis (AREA)
Abstract
The invention discloses a vehicle-mounted all-around online SLAM system and a method based on a multi-camera model, wherein the system comprises a plurality of camera modules, a plurality of cameras and a plurality of display modules, wherein the plurality of cameras are arranged in different directions of a vehicle and are used for acquiring image information of the surrounding environment of the vehicle; the data input module establishes a multi-camera model according to the input image information and the motion parameters of the vehicle, and estimates and positions the motion of the vehicle by using a multi-camera SLAM algorithm; the multi-camera angular point tracking module carries out angular point tracking on the image information, carries out initial attitude estimation and repositioning on the vehicle, tracks a local map of the vehicle, and analyzes and judges whether a new key frame is inserted or not; a rapid pose calculation module fusing the IMU and the wheel speed meter performs combined optimization on the motion parameters and the image information of the vehicle in a sliding window to obtain the pose of the vehicle; the multi-camera closed-loop detection module detects whether the vehicle is closed-loop according to the track of the vehicle and corrects the closed-loop to obtain the vehicle track closest to the true value.
Description
Technical Field
The invention belongs to the technical field of computer vision detection, and particularly relates to a vehicle-mounted all-round-looking online SLAM system and method based on a multi-phase machine model.
Background
With the rapid development of artificial intelligence, the fields of robots, unmanned vehicles and the like become research hotspots of numerous scientific research technicians, and particularly under the conditions of unknown environment and incapability of using GPS signals, the independent realization of accurate positioning and map creation are the research preconditions in the field. Synchronous positioning and Mapping (SLAM) refers to a method for sensing and constructing a consistent environment map by using a self or external sensor and determining the relative posture of the self under the condition that the posture of a mobile platform is unknown. SLAM, as a key to enabling mobile platform autonomy, has shown tremendous utility in many areas. In order to achieve standardization and generalization of application scenarios, a robust real-time SLAM system is required.
The automatic driving automobile is a complex intelligent system, and a lot of technical challenges are faced to realize automatic driving, and motion estimation is one key technology. In motion estimation, various sensors (such as a camera, a laser radar, an inertial measurement unit IMU, a wheel speed meter, etc.) are often used, wherein a vision-based method, which mainly relies on image information provided by a vehicle-mounted camera, is widely used in motion estimation technology due to the advantages of light weight, low cost, sufficient information that can be provided, and the like of the dependent sensors (generally, a camera, an inertial measurement unit, a wheel speed meter, etc.). In addition, the multi-camera system can cover a wider field of view, so that the performance of motion estimation is improved, and the multi-camera system has more advantages particularly in an environment with poor texture. Some motion estimation methods for multi-camera systems have been developed so far, such as extending the existing mainstream SLAM algorithms (such as ORB-SLAM2 and DSO) to multi-camera systems, but these methods have a problem of requiring a huge calculation cost. However, existing pose algorithms are either applicable to single camera systems, make simplistic assumptions, are computationally expensive, or simply degrade under incomplete vehicle motion. Therefore, the computer vision technology is not robust enough, and due to the limitation of the prior art, the vehicle-mounted sensor for automatic driving cannot sufficiently sense and acquire the surrounding environment information (for example, objects with white backgrounds cannot be identified), which still has great potential safety hazard for automatic driving of automobiles.
Therefore, how to provide a stable and reliable visual SLAM system to solve the problems of insufficient information acquisition by an environment perception method and guarantee of the accuracy and stability of the visual SLAM system in the prior art is still a key problem to be researched in the field of automatic driving.
Disclosure of Invention
The invention aims to provide a vehicle-mounted all-round-looking online SLAM system and a method based on a multi-camera model, which aim to overcome the defects of the prior art and provide effective and accurate environmental information for ground environmental perception and positioning navigation of an automatically-driven vehicle, wherein the four cameras are arranged on the vehicle to process received images to construct a visual model in four directions, angular points are tracked by a KLT optical flow algorithm, an inertial measurement unit IMU and a wheel speed meter are fused with visual information, and the offset of a sensor is estimated in a tightly-coupled sliding window optimization frame to realize online real-time all-round-looking camera visual SLAM and space motion tracking, so that the defects that the vehicle motion estimation of the all-round-looking camera in the prior art is not robust, and a vehicle-mounted computing center cannot process in real time due to the requirement of a large amount of computing resources so that a user cannot use the vehicle-mounted computing center in the driving process in an online real time manner are solved, the safety of automatic driving is guaranteed.
In order to achieve the above objects and other related objects, the present invention provides the following technical solutions: a vehicle-mounted all-round-looking online SLAM system based on a multi-camera model comprises a multi-camera module, a data input module, a multi-camera angular point tracking module, a rapid pose calculation module fusing an IMU and a wheel speed meter, and a multi-camera closed loop detection module, wherein the multi-camera module is arranged in different directions of a vehicle by adopting a plurality of cameras and is used for acquiring image information of the surrounding environment of the vehicle; the data input module is used for inputting and establishing a multi-camera model according to input image information and motion parameters of the vehicle, projecting scene points to a coordinate system of the multi-camera model, and estimating and positioning the motion of the vehicle by using a multi-camera SLAM algorithm; the multi-camera corner tracking module is used for carrying out corner tracking on the image information input in the data input module, carrying out initial attitude estimation and repositioning on the vehicle, tracking a local map of the vehicle and analyzing and judging whether a new key frame is inserted or not; a rapid pose calculation module fusing the IMU and the wheel speed meter, and performing combined optimization on the motion parameters and the image information of the vehicle in a sliding window to obtain the pose of the vehicle; and the multi-camera closed-loop detection module is used for obtaining the track of the vehicle according to the local map of the vehicle and the pose information of the vehicle, detecting whether the track of the vehicle is closed-loop or not, and correcting the current detection result to obtain the vehicle track closest to the true value.
Further, the vehicle motion parameters comprise real-time three-axis acceleration, three-axis angular velocity and speed of the vehicle acquired by a wheel speed meter, wherein the real-time three-axis acceleration and the three-axis angular velocity are acquired by the inertial measurement unit IMU.
Furthermore, the system also comprises an external parameter optimization module which optimizes the external parameters of the camera according to the received vehicle motion parameters and the image information.
Further, a key frame contains images acquired by multiple cameras simultaneously.
Further, the camera model comprises: the system comprises a pinhole camera model, an OPENCV fisheye camera model, a Skala Murray camera model and a Kannala Borand camera model, wherein different distortion removal and projection are adopted by different camera models.
The invention also provides a vehicle-mounted all-round online SLAM method based on the multi-phase machine model, which comprises the following steps: initializing in a multi-camera module, establishing a panoramic camera model, projecting scene points into a multi-camera coordinate system, and configuring each camera in a vehicle-mounted panoramic camera into the multi-camera coordinate system in a fixed direction;
receiving image information acquired by the vehicle-mounted all-around camera through a data input module, and fusing pictures of different cameras by using multiple key frames;
thirdly, a rapid pose calculation module fusing the IMU and the wheel speed meter receives data information acquired by an inertial measurement unit IMU and the wheel speed meter of the vehicle in the data input module, and performs combined optimization calculation on the image information and the data information in a sliding window to finally obtain the pose of the vehicle, wherein the pose of the vehicle is integrated to form a vehicle track;
and step four, detecting whether the vehicle track is closed-loop in the multi-camera closed-loop detection module, and correcting the current result to obtain the vehicle track closest to the true value.
Further, in the first step, according to the speed information acquired by the wheel speed meter, the pose of each frame is obtained by using pre-integration, and then the poses of the ten frames are used for mutually triangulating the front feature points to form local map points, so as to complete initialization.
Further, in the second step, after the camera collects a new image, an optical flow algorithm is called to track the previous corner points into the current image, the corner points which fail to track are removed according to the tracking state, and the corner points at the edge of the tracked image are removed by utilizing edge judgment.
Further, in the second step, one multi-key frame includes images simultaneously acquired by a plurality of cameras, and the images simultaneously acquired by the plurality of cameras are placed in the key frame as one frame, that is, the one-frame key frame includes information of four cameras.
Further, in the fourth step, the DBoW2 is adopted to detect whether the vehicle track has closed loop, the most similar frame is found from the word packet database through the DBoW2, the closed loop frame is added into the sliding window, and the repositioning result is calculated by using joint optimization.
By adopting the technical scheme of the invention, the method has the beneficial effects that: the multi-camera system has the advantages that the sensing range is expanded by adopting the multi-camera system to position the current vehicle, various sensors (such as a plurality of cameras, an inertia measuring unit and a wheel speed meter) are introduced to improve the robustness of the system, the problems of small field angle and limited field of view of the existing vision SLAM method can be solved, the robustness is improved, the calculation speed is improved, and the reliability guarantee is provided for the autonomy of the robot, so that the autonomy navigation sufficiency of the robot is ensured, and the system is safer and more reliable in practical application.
Drawings
FIG. 1 is a schematic diagram of a vehicle-mounted all-around online SLAM system framework based on a multi-phase machine model.
FIG. 2 is a schematic diagram of the communication relationship between nodes in the operation process of the present invention.
Detailed Description
Embodiments of the present invention will be described in further detail below with reference to the accompanying drawings, so that the technical solutions can be more clearly understood. Other advantages and effects of the present invention will be readily apparent to those skilled in the art from the disclosure herein. The invention is capable of other and different embodiments and its several details are capable of modifications and variations in various respects, all without departing from the spirit of the invention. It is to be noted that the features in the following embodiments and examples may be combined with each other without conflict.
Fig. 1 is a schematic diagram of a framework of a vehicle-mounted all-around online SLAM system based on a multiphase model. The system comprises a data input module, a multi-camera angular point tracking module, a rapid pose calculation module fusing an IMU and a wheel speed meter, and a multi-camera closed loop detection module.
In the multi-camera closed-loop detection module, a plurality of cameras are arranged in different directions of a vehicle and used for acquiring image information of the surrounding environment. The system adopts 4 cameras, is configured in the forward-looking, left-looking, rear-looking and right-looking directions on the vehicle, and is used for acquiring the real-time vision of 360 degrees around the vehicle body to detect and identify the surrounding environment of the vehicle in real time.
In the data input module, the information input to the data input module includes: the image information of the surrounding environment of the vehicle is acquired by the multi-camera module, the angular velocity and linear acceleration information are acquired by the inertial measurement unit IMU, and the speed information is acquired by the wheel speed meter. And establishing a multi-camera model through the information input into the data input module, projecting the scene points to a coordinate system of the multi-camera model, and estimating and positioning the vehicle motion by using a multi-camera SLAM algorithm.
The multi-camera corner tracking module is used for carrying out corner tracking on the image information of the cameras input in the data input module, carrying out initial attitude estimation and repositioning on the vehicle, tracking a local map of the vehicle, determining whether to insert a new key frame into the sliding window according to whether a new frame is the key frame, and inserting the new key frame into the sliding window if the new frame is the key frame; if the new frame is not a key frame, no insertion is made into the sliding window. The method for judging whether the key frame is the key frame comprises the following steps: when a new frame of image is acquired, the relative displacement between the frame and the previous frame stored in the sliding window, namely the relative position between the frame and the corner of the tenth frame, is firstly judged and expressed by the relative displacement of the corner. If the average relative displacement of the angular points is larger than a set threshold value, judging that the frame is a new key frame, pressing the new key frame into the latest position in the sliding window, moving other key frames forwards, and marginalizing the first key frame; if the key frame is not new, the previous tenth frame is rimmed and the new frame is replaced by the tenth frame. In the SLAM system, the map is divided into a local map and a global map, the local map used here describes the information of the feature points near the vehicle body, in the process of selecting the feature points, only the feature points near the current position of the camera are reserved, and the feature points far away or outside the field of view are removed, so that a small-range local map is established.
The fast pose calculation module fusing the IMU and the wheel speed meter is used for acquiring the angular velocity, the linear acceleration and the speed of a vehicle in the motion process by the inertial measurement unit IMU and the wheel speed meter, image information acquired by the multi-camera module is input into a program through the data input module, and the IMU, the wheel speed meter and the image information are jointly optimized in a sliding window to obtain the pose of the vehicle.
The multi-camera closed-loop detection module is used for obtaining a vehicle track by obtaining a vehicle local map from the multi-camera angular point tracking module and obtaining vehicle pose information from the fast pose calculation module fusing the IMU and the wheel speed meter, and is used for detecting whether the vehicle track is closed loop or not and correcting the current detection result to obtain the vehicle track closest to a true value.
The vehicle motion parameters include real-time three-axis acceleration, three-axis angular velocity and speed of the vehicle collected by a wheel speed meter, which are collected by an inertial measurement unit IMU.
The system also includes an external parameter optimization module that optimizes external parameters (i.e., camera external parameters) based on the received data information (vehicle motion parameters and image information). The camera external parameters specifically refer to rotation and translation parameters of the camera.
The camera model of the present embodiment may be selected from the following types: the system comprises a pinhole camera model, an OPENCV fisheye camera model, a Skala Murray camera model and a Kannala Borand camera model, wherein different distortion removal and projection are adopted by different camera models.
Corresponding to the system, the embodiment also relates to a vehicle-mounted all-around online SLAM method based on the multi-camera model, which comprises the following steps:
initializing in a multi-camera module, establishing a panoramic camera model, projecting scene points into a multi-camera coordinate system, and configuring each camera in a vehicle-mounted panoramic camera into the multi-camera coordinate system in a fixed direction;
receiving image information of a panoramic camera input by a multi-camera module of a vehicle through a data input module, and fusing pictures of different cameras by using multiple key frames;
thirdly, a rapid pose calculation module fusing the IMU and the wheel speed meter receives data information acquired by an inertial measurement unit IMU and the wheel speed meter of the vehicle in the data input module, and performs combined optimization calculation on the image information and the data information in a sliding window to finally obtain the pose of the vehicle, wherein the pose of the vehicle is integrated to form a vehicle track;
and step four, detecting whether the vehicle track is closed-loop in the multi-camera closed-loop detection module, and correcting the current result to obtain the vehicle track closest to the true value.
In the first step, according to more accurate speed information acquired by the wheel speed meter, the pose of each frame is obtained by pre-integration in the previous ten frames, and then the poses of the ten frames are used for mutually triangularizing the previous feature points to form local map points, so that initialization is completed.
In the second step, after the camera collects a new image, an optical flow algorithm is called to track the previous corner points into the current image, the corner points which fail in tracking are removed according to the tracking state, and the corner points of the tracked image edge are removed by utilizing edge judgment. The key frame comprises images acquired by a plurality of cameras at the same time, and the images acquired by the plurality of cameras at the same time are taken as one frame and placed in the key frame.
In the fourth step, DBoW2 (binary word package for FAST position recognition in image sequence, which is replaced by DBoW2 for simplicity) is used to detect whether the vehicle track is closed loop, and for each frame key frame, we extract 500 FAST corners and BRIEF descriptors and convert the descriptors into word package vectors, and add them to the word package database. The most similar frames are found from the package database by DBoW2, the closed-loop frames are added to the sliding window, and the relocation result is calculated using joint optimization.
According to the vehicle-mounted all-round-looking online SLAM method based on the multi-camera model, corresponding camera models are determined according to different used cameras, all-round-looking camera models are established, pose estimation is carried out on a vehicle, and multiple key frames are used in the all-round-looking camera online SLAM algorithm to fuse pictures of the different cameras; during the operation of the system, the external parameter optimization module can be turned on or off by manually adjusting the turning-on or turning-off parameters of the external parameter optimization module (for example, if i want to turn on the module, the flag position is True, and the turning-off is False, and the parameter is the flag position), so as to determine whether to use the external parameter optimization module, and if the external parameter optimization is turned on, the system not only uses the given external parameters (namely, the camera external parameters) but also continuously optimizes the external parameters (namely, the camera external parameters) according to the received information in the processing thread; if the external parameter optimization module is turned off, the system will always use the given external parameters. In the process of running of an experimental vehicle, image information of the surrounding environment of the vehicle is collected by a multi-camera module, image information of four cameras, three-axis acceleration and three-axis angular velocity information collected by an inertial measurement unit and speed information collected by a wheel speed meter are received by a data input module and then input to a rapid measurement module fusing an IMU and the wheel speed meter, the image information obtained by the IMU, the wheel speed meter and a look-around camera is fused by a rapid pose calculation module fusing the IMU and the wheel speed meter, joint optimization is carried out in a sliding window, closed loop detection of the multi-camera is carried out after the track of the vehicle is obtained, whether the track is closed or not is detected, if closed loop occurs, track optimization is carried out in the multi-camera closed loop detection module, and finally the pose of the experimental vehicle is obtained. The method comprises the following specific steps:
(1) establishing different multi-camera models according to different camera models and different distortion models, projecting scene points to a multi-camera coordinate system, and configuring each vehicle-mounted camera in the multi-camera coordinate system in the forward-looking direction, the left-looking direction, the rear-looking direction and the right-looking direction; the multi-camera SLAM algorithm proposed herein is utilized for vehicle motion estimation and localization, multi-camera corner tracking is performed in the algorithm, and multiple key frames are used to fuse pictures of different cameras. The method specifically comprises the following steps:
(1.1) establishing a camera model process: the selectable camera models in the system comprise a PINHOLE PINHOLE camera model, an OPENCV fisheye camera model, a Skala Murray camera model and a Kannala Bolat camera model, the corresponding camera models are selected according to the types of the vehicle-mounted panoramic cameras, and different distortion removal and projection modes are adopted by different camera models. Such as: if a fisheye camera is used, an OPENCV fisheye camera model or a fisheye camera model can be selected, and the fisheye camera model projects 3D points onto a unit spherical surface first, and then projects the points on the unit spherical surface onto a normalization plane.
When the fisheye camera model is selected, the shadow mask is used for removing the effective image only in the middle circle part of the image. And extracting new angular points in the area which is not removed, so that no matter how many angular points are tracked before, the new angular points can be added subsequently, and the image of each frame has the number of the angular points meeting the requirement.
(1.2) multi-camera corner point tracking thread: the method is used for acquiring image information of the surrounding environment of the vehicle in the running process of the vehicle, and the image information is realized by four cameras in a surrounding view (namely front view, left view, rear view and right view). The thread performs corner tracking on the images of the cameras without adopting a method for extracting feature points, so that the speed of front-end corner tracking can be increased. And performing initial attitude estimation or repositioning through angular point tracking, tracking a local map, and finally judging to insert new multiple key frames. The multi-key frame refers to a key frame which contains images simultaneously acquired by a plurality of cameras, and four frames of images acquired at the same time are regarded as one frame in the algorithm. The multi-camera corner tracking thread steps as follows.
The method comprises the following steps: and a callback function is set in the thread to receive the image of each camera, and histogram equalization is performed on the image of each camera, so that the overall exposure of the image is more balanced, and the situation that a certain part is too bright or too dark can not occur.
Step two: and if the established camera model is the fisheye camera model, adding an elliptical mask to the image. Because only the image in the middle part area of the image is an effective part, the multi-camera corner point tracking module adds masks to the images received by the four cameras to remove peripheral ineffective parts in the images acquired by the fisheye cameras, wherein the elliptical masks are manually set elliptical masks; if the fish-eye camera model is not adopted, the corresponding elliptical mask is not added.
Step three: and (2) performing KLT algorithm tracking corner points on the image processed in the first step on each camera, eliminating corner points with failed tracking according to a tracking state (obtained by utilizing a built-in function of OPENCV) calculated by a five-point-based essence matrix algorithm in the openCV, and eliminating the corner points at the edge of the image, namely the part tracked to the pixel value of 0 in a mask, through a pre-set binary elliptical mask.
Step four: and (4) calculating the tracking matching pair obtained in the step two (the matching pair obtained by using an opencv built-in function) by using RANSAC and an eight-point method, and removing the outlier.
Step five: the tracking times of each angular point are judged, the tracked angular points are sorted according to the tracking times, then the angular points are sequentially selected, one angular point is selected, a region in a preset radius with the angular point as the center in the mask is set to be 0, and the angular point in the region is not selected any more later, so that the tracked angular points can be kept in a more uniformly distributed state.
Step six: and extracting new corner points in the area which is not 0 in the mask, wherein the number of the extracted corner points is the number obtained by subtracting the number obtained by last tracking from the number which is initially set, so that the new corner points can be added subsequently regardless of the number of the tracked corner points before the operation, and the frame of the common single image has the number of the corner points which meets the requirement.
Step seven: after the one-step processing process is completed, the position information of the corner points tracked by each camera and the tracking state are updated, wherein the tracking state refers to the times of tracking and not tracking, and tracking if any.
In addition, in the thread, whether the image after the distortion removal is normal (that is, the visualized image looks undistorted with naked eyes) can be observed by adjusting parameters (for example, if i want to turn on the module, the flag position is set to True, and the parameter is the flag position), if the node displaying the image after the distortion removal is turned on, the image after the distortion removal of different camera models according to different distortion models can be seen in the program running process, and if the node is turned off, the image after the distortion removal cannot be seen, but correspondingly, the processing speed of the corner tracking module is improved to a certain extent.
(2) When receiving a low-frequency camera image, the system simultaneously receives information of a high-frequency inertial processing unit IMU and a wheel speed meter, and is provided with an IMU and wheel speed meter fused rapid pose calculation module for receiving motion parameters in the running process of a vehicle, wherein the motion parameters comprise real-time three-axis acceleration and three-axis angular velocity acquired by the IMU equipped for the vehicle, and speed information acquired by the wheel speed meter. In the pose calculation integrating the IMU and the wheel speed meter, after initialization, in order to reduce the calculated amount, the angular velocity and linear acceleration information acquired by the inertial measurement unit, the velocity information acquired by the wheel speed meter and the image information of the surrounding environment of the vehicle acquired by the multi-camera module are directly subjected to combined optimization calculation, a sliding window is used for optimizing all the variables in the sliding window, and external parameter optimization is carried out. Receiving a new picture with tracked corner points in a tracking process processing thread, calculating coordinates of the corner points, creating map points, judging whether marginalization is needed in a sliding window, judging whether a new frame of image is a key frame, and maintaining the sliding window. Some methods and details in the above process are as follows:
(2.1) an initialization method: because the wheel speed meter can obtain more accurate scale information in a short time, in the initial ten frames received by the data input module, each frame uses pre-integration to obtain a pose, and then the poses of the ten frames are used for mutually triangularizing front corner points to form local map points, so that the initialization process is completed.
(2.2) tracking process: in the tracking process, each time a new picture with the tracked corner points is received by the processing thread, for the corner points which are not deep yet in the tracked corner points, the coordinates of the corner point are calculated by using a singular value decomposition algorithm, so that the reprojection error of the corner point on each frame of image observed by the processing thread is minimum. The coordinates of the point are expressed in depth of the camera coordinate system of the first frame image in which the corner point is observed. And optimizing by combining the pose of each key frame, the appearance of each camera, the marginalized information, the error of pre-integration, the reprojection error of each corner point and the loop or closed-loop error. In the sliding window, the condition for determining marginalization is whether a new key frame comes in. If there is marginalization, the first key frame in the window is slid off. The corner points first observed by this key frame are then transferred to the new 0 th key frame. If there is no marginalization, the previous latest frame is replaced with the incoming latest frame.
(2.3) method for judging whether the key frame is the key frame: when a new frame of image is acquired, the relative displacement between the frame and the previous frame stored in the sliding window, namely the relative position between the frame and the corner of the tenth frame, is firstly judged and expressed by the relative displacement of the corner. If the average relative displacement of the angular points is larger than a set threshold value, judging that the frame is a new key frame, pressing the new key frame into the latest position in the sliding window, moving other key frames forwards, and marginalizing the first key frame; if the key frame is not new, the previous tenth frame is rimmed and the new frame is replaced by the tenth frame.
(2.4) a method of creating map points: for those corner points that have been tracked and have not yet been deep, if it has been observed by more than two previous key frames, its coordinates are calculated using singular value decomposition to minimize its reprojection error on each frame of image observed. The coordinates of a point are represented by the depth of the camera coordinate system in the first frame image it is viewed at.
(2.5) an external parameter optimization module: the goal of the optimization is the position, pose, offset, camera outlook of each frame and the depth of the first frame each corner point is observed at. The optimized residual errors include four items, namely a priori residual error, an IMU (inertial measurement Unit) residual error, a wheel speed meter residual error, a reprojection residual error and a closed loop optimization residual error. And adding the four residual error items to obtain a final residual error item, and then optimizing by using a Levenberg optimization method. After optimization, the pose of each frame in the sliding window and the inverse depth of the corner point in each local map are obtained, and therefore the current pose and the inverse depth of the corner point are updated. The method for optimizing the residual error comprises the following steps: after each nonlinear iteration, the value of the independent variable is adjusted according to the residual error and the Jacobian, a new residual error is calculated according to the adjusted independent variable, and then a new Jacobian is calculated. And finally, optimizing the final residual error item by using a Levenberg optimization method.
(2.6) in the process of vehicle motion, the condition of vehicle track closed loop may occur, and a multi-camera closed loop detection module is arranged in the system and used for detecting whether the track is closed loop or not and correcting the current result to enable the track to be closer to the true value. In the module, firstly, a DBoW2 (binary word package for fast position recognition in an image sequence, hereinafter, for simplicity, the DBoW2 is used for replacement) is used for closed-loop detection, a closed-loop frame is detected, relocation is performed, and finally global pose optimization is performed, wherein in the global pose optimization process, a pose graph needs to be managed.
(2.7) closed-loop detection method: the closed-loop detection module adopts DBoW2 to perform closed-loop detection, finds the most similar frame from the data through DBoW2, finds the most similar ten frames as closed-loop candidate frames, wherein the most similar frame is a closed-loop frame, adds the closed-loop frame into a sliding window, and calculates a relocation result by using joint optimization. In the closed-loop detection process, the association between the current frame and the closed-loop candidate frame is verified through the corner points. First, the corner matching of the two frames is performed through a BRIEF descriptor, and the matching pair with the shortest distance is selected through a Hamming distance. Direct matching can cause a plurality of outlier matching, and in order to remove the outlier matching, geometric verification is carried out by adopting two steps of a.2D-2D, calculating a basic matrix by using RANSAC (random sample consensus algorithm) to eliminate matching between 2D-2D; and b, 3D-2D, using RANSAC and PnP (abbreviation of passive-n-Point), which is a method for solving the motion of a 3D-to-2D Point pair, to carry out matching verification between the 3D Point and the 2D Point. The matching pairs that pass the above two verifications are added to the candidate keyframes.
(2.8) the relocation method: and adding the closed-loop frame into a local sliding window, and fixing the pose of the closed-loop frame and the 3D point observed by the closed-loop frame. When repositioning, we need to estimate the relative pose of the closed-loop frame v frame and the current frame. All state variables in the sliding window at this time, including point and pose and IMU and wheel speed meter information, are jointly optimized. At this time, the observation equation in the closed-loop frame and the current sliding window in the visual observation model can be easily written, and the nonlinear error is as follows:
the relocation result calculated using this joint optimization is more accurate. This result is then used to calculate the relative pose, i.e. the current offset error, between the closed-loop frame and the current frame.
(2.9) the global pose optimization method comprises the following steps: after relocation, a track capable of representing the global pose is obtained, global pose optimization is performed by using a lightweight 4-degree-of-freedom global pose graph optimization method, and vehicle tracks which are likely to have drift or jump changes can be smoother and more consistent. Because the IMU is influenced by the direction of gravity, roll and pitch angles are observed completely in the visual inertial system, and because roll and pitch angle motions are very small in the vehicle body motion, only x, y, z and yaw angles are estimated, and the global pose can be optimized. Each frame of key frame is added into the pose graph after being edged, and one key frame is used as a vertex in the pose graph of the key frame. By optimizing the pose graph, the vertex is adjusted, and the configuration is matched with the edge as much as possible. Only the x, y, z and yaw ψ of the 3D locating vertex are adjusted, and the roll angle and pitch angle are set as constant variables. Such an optimization does not correct in the drift-free direction. The residual edges are as follows:
(2.10) a management method of the pose graph: when the driving distance increases, the size of the pose graph may increase infinitely, thereby limiting the real-time performance of the system, so the pose graph in (3.3) needs to be managed. To do this, a downsampling process is implemented to preserve the pose graph size, taking the pose graph to a finite size. All key frame closing constraints with closed loops will remain, while other key frames are either too close or neighboring frames with very similar directions are removed.
(2.11) according to the framework shown in the attached figure 1, a vehicle-mounted all-round-looking online SLAM system based on a multi-phase machine model is built, input information of a high-frequency inertia measurement unit IMU and a wheel speed meter is fused to perform rapid pose calculation, meanwhile, low-frequency image information is received through a callback function and then enters a multi-camera corner point tracking module to perform KLT optical flow tracking (the corner point tracking and the IMU wheel speed meter information fusion of the system and the method are parallel in multiple threads).
After image information of a camera, three-axis acceleration and three-axis angular velocity information of an inertial measurement unit IMU and speed information of a vehicle acquired by a wheel speed meter are received for the first time, a system is prepared to enter an initialization process, and after initialization, multi-sensor combined optimization is carried out in a sliding window. The low-frequency image information received by the system is converted into a word packet vector and stored into a word packet, whether the track is closed-loop or not is detected by a closed-loop detection module, and the current result is corrected, so that the track is closer to a true value. And performing pose calculation on the result of the closed loop and the optimization result in the sliding window, sending the pose obtained by calculation to a pose graph optimization module, and optimizing to obtain the final global pose.
Fig. 2 is a schematic diagram of a communication node in the implementation process of the present invention, and a data receiving module receives image topics of four cameras: (iii)/camera/0/0/image,/camera/0/1/image,/camera/0/2/image,/camera/0/3/image and topics that merge inertial measurement units IMU and wheel speed meters: the method comprises the steps of performing multi-camera corner tracking by four camera image entry/multi _ feature _ tracker threads, outputting a tracking result to/camera/0/0/feature, performing joint optimization in a sliding window/odometry _ kl _ test node together with the four camera image entry/multi _ feature _ tracker threads, performing closed-loop detection and pose graph management by the entry/pos _ graph node, and finally outputting a track in the/pos _ graph topic.
The above embodiments are the best mode for carrying out the invention, but the embodiments of the invention are not limited to the above embodiments, and any other changes, modifications, substitutions, combinations, and simplifications which do not depart from the spirit and principle of the invention should be regarded as equivalent substitutions, which are included in the scope of the present invention. More specifically, various variations and modifications are possible in the component parts and/or arrangements of the subject combination arrangement within the scope of the disclosure, the drawings and the appended claims. In addition to variations and modifications in the component parts and/or arrangements, other uses will also be apparent to those skilled in the art.
Claims (10)
1. A vehicle-mounted looking-around online SLAM system based on a multi-camera model is characterized by comprising a multi-camera module, a data input module, a multi-camera angular point tracking module, a rapid pose calculation module fusing an IMU and a wheel speed meter, and a multi-camera closed loop detection module, wherein,
the multi-camera module is arranged in different directions of the vehicle by adopting a plurality of cameras and is used for acquiring image information of the surrounding environment of the vehicle;
the data input module is used for inputting and establishing a multi-camera model according to input image information and motion parameters of the vehicle, projecting scene points to a coordinate system of the multi-camera model, and estimating and positioning the motion of the vehicle by using a multi-camera SLAM algorithm;
the multi-camera corner tracking module is used for carrying out corner tracking on the image information input in the data input module, carrying out initial attitude estimation and repositioning on the vehicle, tracking a local map of the vehicle and analyzing and judging whether a new key frame is inserted or not;
a rapid pose calculation module fusing the IMU and the wheel speed meter, and performing combined optimization on the motion parameters and the image information of the vehicle in a sliding window to obtain the pose of the vehicle;
and the multi-camera closed-loop detection module is used for obtaining the track of the vehicle according to the local map of the vehicle and the pose information of the vehicle, detecting whether the track of the vehicle is closed-loop or not, and correcting the current detection result to obtain the vehicle track closest to the true value.
2. The multi-camera model-based vehicle-mounted looking-around online SLAM system as claimed in claim 1, wherein the vehicle motion parameters comprise real-time three-axis acceleration, three-axis angular velocity and wheel speed of the vehicle collected by an inertial measurement unit IMU.
3. The multi-camera model-based vehicle-mounted all-around online SLAM system of claim 2, further comprising an external parameter optimization module for optimizing external parameters of the camera according to received vehicle motion parameters and image information.
4. The multi-camera model-based vehicle-mounted all-around online SLAM system of claim 1, wherein a key frame comprises images acquired by a plurality of cameras simultaneously.
5. The multi-camera model-based vehicle-mounted look-around online SLAM system of claim 1, wherein the camera model comprises: the system comprises a pinhole camera model, an OPENCV fisheye camera model, a Skala Murray camera model and a Kannala Borand camera model, wherein different distortion removal and projection are adopted by different camera models.
6. A vehicle-mounted all-round-looking online SLAM method based on a multi-camera model comprises the following steps:
initializing in a multi-camera module, establishing a panoramic camera model, projecting scene points into a multi-camera coordinate system, and configuring each camera in a vehicle-mounted panoramic camera into the multi-camera coordinate system in a fixed direction;
receiving image information acquired by the vehicle-mounted all-around camera through a data input module, and fusing pictures of different cameras by using multiple key frames;
thirdly, a rapid pose calculation module fusing the IMU and the wheel speed meter receives data information acquired by an inertial measurement unit IMU and the wheel speed meter of the vehicle in the data input module, and performs combined optimization calculation on the image information and the data information in a sliding window to finally obtain the pose of the vehicle, wherein the pose of the vehicle is integrated to form a vehicle track;
and step four, detecting whether the vehicle track is closed-loop in the multi-camera closed-loop detection module, and correcting the current result to obtain the vehicle track closest to the true value.
7. The on-vehicle looking-around online SLAM method based on the multi-camera model as claimed in claim 6, wherein in the first step, based on the speed information collected by the wheel speed meter, the poses of each frame are obtained by pre-integration, and then the poses of the ten frames are used to mutually triangulate the previous feature points to form local map points, thereby completing initialization.
8. The on-vehicle looking-around online SLAM method based on the multi-camera model as claimed in claim 6, wherein in the second step, after the camera collects a new image, an optical flow algorithm is invoked to track previous corner points into the current image, corner points which fail to track are removed according to the tracking state, and corner points of the tracked image edge are removed by edge judgment.
9. The on-vehicle all-around online SLAM method based on the multi-camera model as claimed in claim 6, wherein in the second step, the images obtained by the multiple cameras simultaneously are included in a multi-key frame, and the images obtained by the multiple cameras simultaneously are placed in the key frame as a frame.
10. The method of claim 6, wherein in the fourth step, DBoW2 is used to detect whether the vehicle track is closed loop, the most similar frame is found from the word bag database through DBoW2, the closed loop frame is added into the sliding window, and joint optimization is used to calculate the repositioning result.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111120380.9A CN113959435A (en) | 2021-09-24 | 2021-09-24 | Vehicle-mounted all-around online SLAM system and method based on multi-camera model |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111120380.9A CN113959435A (en) | 2021-09-24 | 2021-09-24 | Vehicle-mounted all-around online SLAM system and method based on multi-camera model |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN113959435A true CN113959435A (en) | 2022-01-21 |
Family
ID=79462000
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111120380.9A Pending CN113959435A (en) | 2021-09-24 | 2021-09-24 | Vehicle-mounted all-around online SLAM system and method based on multi-camera model |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113959435A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115272494A (en) * | 2022-09-29 | 2022-11-01 | 腾讯科技（深圳）有限公司 | Calibration method and device for camera and inertial measurement unit and computer equipment |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109506642A (en) * | 2018-10-09 | 2019-03-22 | 浙江大学 | A kind of robot polyphaser vision inertia real-time location method and device |
| CN109509230A (en) * | 2018-11-13 | 2019-03-22 | 武汉大学 | A kind of SLAM method applied to more camera lens combined type panorama cameras |
| CN110070615A (en) * | 2019-04-12 | 2019-07-30 | 北京理工大学 | A kind of panoramic vision SLAM method based on polyphaser collaboration |
| CN112862818A (en) * | 2021-03-17 | 2021-05-28 | 合肥工业大学 | Underground parking lot vehicle positioning method combining inertial sensor and multi-fisheye camera |
-
2021
- 2021-09-24 CN CN202111120380.9A patent/CN113959435A/en active Pending
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109506642A (en) * | 2018-10-09 | 2019-03-22 | 浙江大学 | A kind of robot polyphaser vision inertia real-time location method and device |
| CN109509230A (en) * | 2018-11-13 | 2019-03-22 | 武汉大学 | A kind of SLAM method applied to more camera lens combined type panorama cameras |
| CN110070615A (en) * | 2019-04-12 | 2019-07-30 | 北京理工大学 | A kind of panoramic vision SLAM method based on polyphaser collaboration |
| CN112862818A (en) * | 2021-03-17 | 2021-05-28 | 合肥工业大学 | Underground parking lot vehicle positioning method combining inertial sensor and multi-fisheye camera |
Non-Patent Citations (1)
| Title |
|---|
| 张礼廉 等: "视觉/惯性组合导航技术发展综述", 导航定位与授时, vol. 7, no. 04, 31 July 2020 (2020-07-31), pages 50 - 63 * |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115272494A (en) * | 2022-09-29 | 2022-11-01 | 腾讯科技（深圳）有限公司 | Calibration method and device for camera and inertial measurement unit and computer equipment |
| CN115272494B (en) * | 2022-09-29 | 2022-12-30 | 腾讯科技（深圳）有限公司 | Calibration method and device for camera and inertial measurement unit and computer equipment |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111462135B (en) | Semantic mapping method based on visual SLAM and two-dimensional semantic segmentation | |
| CN110322500B (en) | Optimization method and device for instant positioning and map construction, medium and electronic equipment | |
| CN109993113B (en) | Pose estimation method based on RGB-D and IMU information fusion | |
| US10948297B2 (en) | Simultaneous location and mapping (SLAM) using dual event cameras | |
| CN112734852B (en) | Robot mapping method and device and computing equipment | |
| Rambach et al. | Learning to fuse: A deep learning approach to visual-inertial camera pose estimation | |
| WO2017163596A1 (en) | Autonomous navigation using visual odometry | |
| JP6767998B2 (en) | Estimating external parameters of the camera from the lines of the image | |
| CN111210477B (en) | Method and system for positioning moving object | |
| CN112233177B (en) | Unmanned aerial vehicle pose estimation method and system | |
| CN116205947B (en) | Binocular-inertial fusion pose estimation method based on camera motion state, electronic equipment and storage medium | |
| CN105205459B (en) | A kind of recognition methods of characteristics of image vertex type and device | |
| CN110349212B (en) | Optimization method and device for instant positioning and map construction, medium and electronic equipment | |
| CN111986261B (en) | Vehicle positioning method and device, electronic equipment and storage medium | |
| CN111932674A (en) | Optimization method of line laser vision inertial system | |
| Muñoz-Bañón et al. | Targetless camera-LiDAR calibration in unstructured environments | |
| Michot et al. | Bi-objective bundle adjustment with application to multi-sensor slam | |
| US20200098115A1 (en) | Image processing device | |
| CN117367427A (en) | Multi-mode slam method applicable to vision-assisted laser fusion IMU in indoor environment | |
| Xian et al. | Fusing stereo camera and low-cost inertial measurement unit for autonomous navigation in a tightly-coupled approach | |
| CN113959435A (en) | Vehicle-mounted all-around online SLAM system and method based on multi-camera model | |
| Panahandeh et al. | Vision-aided inertial navigation using planar terrain features | |
| CN112731503A (en) | Pose estimation method and system based on front-end tight coupling | |
| Wang et al. | LF-VISLAM: A SLAM framework for large field-of-view cameras with negative imaging plane on mobile agents | |
| Schill et al. | Estimating ego-motion in panoramic image sequences with inertial measurements |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |