CN113532420B - Visual inertial odometer method integrating dotted line characteristics - Google Patents
Visual inertial odometer method integrating dotted line characteristics Download PDFInfo
- Publication number
- CN113532420B CN113532420B CN202110727116.5A CN202110727116A CN113532420B CN 113532420 B CN113532420 B CN 113532420B CN 202110727116 A CN202110727116 A CN 202110727116A CN 113532420 B CN113532420 B CN 113532420B
- Authority
- CN
- China
- Prior art keywords
- line
- tracking
- frame
- line segment
- characteristic
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 45
- 230000000007 visual effect Effects 0.000 title claims abstract description 37
- 230000004927 fusion Effects 0.000 claims abstract description 12
- 230000010354 integration Effects 0.000 claims abstract description 12
- 238000005457 optimization Methods 0.000 claims abstract description 12
- 230000008878 coupling Effects 0.000 claims abstract description 10
- 238000010168 coupling process Methods 0.000 claims abstract description 10
- 238000005859 coupling reaction Methods 0.000 claims abstract description 10
- 238000000605 extraction Methods 0.000 claims abstract description 7
- 238000005070 sampling Methods 0.000 claims description 23
- 230000003287 optical effect Effects 0.000 claims description 19
- 238000001514 detection method Methods 0.000 claims description 16
- 230000008569 process Effects 0.000 claims description 16
- 239000013598 vector Substances 0.000 claims description 14
- 230000005484 gravity Effects 0.000 claims description 9
- 230000015556 catabolic process Effects 0.000 claims description 6
- 238000006731 degradation reaction Methods 0.000 claims description 6
- 238000012937 correction Methods 0.000 claims description 5
- 238000011084 recovery Methods 0.000 claims description 4
- 238000004364 calculation method Methods 0.000 claims description 3
- 238000012216 screening Methods 0.000 claims description 3
- 230000001131 transforming effect Effects 0.000 claims description 3
- 238000012423 maintenance Methods 0.000 claims description 2
- 230000000717 retained effect Effects 0.000 claims description 2
- 230000000694 effects Effects 0.000 abstract description 12
- 238000010276 construction Methods 0.000 abstract description 9
- 238000012360 testing method Methods 0.000 abstract description 3
- 230000008030 elimination Effects 0.000 abstract 1
- 238000003379 elimination reaction Methods 0.000 abstract 1
- 238000005516 engineering process Methods 0.000 abstract 1
- 230000009466 transformation Effects 0.000 description 6
- 238000011156 evaluation Methods 0.000 description 5
- 238000002474 experimental method Methods 0.000 description 4
- 238000005259 measurement Methods 0.000 description 4
- 238000012545 processing Methods 0.000 description 4
- 230000001360 synchronised effect Effects 0.000 description 4
- 238000012935 Averaging Methods 0.000 description 3
- 238000013507 mapping Methods 0.000 description 3
- 230000008859 change Effects 0.000 description 2
- 238000013519 translation Methods 0.000 description 2
- 230000002159 abnormal effect Effects 0.000 description 1
- 230000003190 augmentative effect Effects 0.000 description 1
- 230000000052 comparative effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000007670 refining Methods 0.000 description 1
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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- 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
-
- 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
- G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Image Analysis (AREA)
Abstract
The invention discloses a visual inertial odometer method integrating dotted line characteristics, and belongs to the field of three-dimensional vision. On the visual inertial odometer system based on the characteristic points, the invention adds a line characteristic tracking module at the front end and realizes a line characteristic management module completely parallel to the point characteristics at the rear end. The front-end line feature tracking module utilizes a front-back frame pre-integration technology to induce the extraction and matching of line features, so that the time consumption for tracking the line features is greatly reduced. The line characteristic management module at the rear end comprises generation and elimination of line characteristics, and the map scale is effectively controlled. In addition, the invention constructs a joint loss function based on the re-projection error of the point line characteristics and the pre-integral error item of the inertial navigation information, and completes the close coupling fusion of the point line characteristics and the inertial navigation information through sliding window optimization. The test result of the invention on the EuRoC data set shows that compared with the original visual inertial odometer system based on the characteristic points, the positioning accuracy is obviously improved, and the line characteristic map construction achieves better effect.
Description
Technical Field
The invention relates to the field of three-dimensional vision, in particular to a visual inertial odometer method integrating dotted line characteristics.
Background
The simultaneous localization and mapping algorithm is an algorithm for simultaneously estimating the pose of the device and constructing an environment map according to continuous motion and environment measurement information in time sequence provided by a sensor in the process of relative motion of the device carrying the sensor and the environment. In the last years, the related work of researchers on visual inertia SLAM and line features can be divided into three aspects of the architecture of visual inertia SLAM, line features and their three-dimensional reconstruction, SLAM systems that fuse line features.
Visual inertial SLAM can be classified into loose coupling and tight coupling according to the fusion manner between sensor information. Under the loose coupling framework, the visual information and the inertial navigation information are respectively and independently calculated by different estimators and are fused into a final result; the same estimator is used under the tightly coupled framework to process both visual and inertial navigation information. Compared with a loose coupling framework, the information fusion is completed in the early stage of estimation by tight coupling, and the method has better estimation precision and robustness.
The detection of line features can be classified into a parameter space method and an image space method according to the detection object. The Hough method is a classical algorithm for line segment detection in a parameter space, which transforms points in an image space into the parameter space, and the extremum detected in the parameter space, i.e. the corresponding straight line segment, is high in computational complexity and limited by the resolution of the parameter space, and combining non-continuous line segments together results in a large number of false detections. The LSD algorithm obtains the seed points of the line segment endpoints based on the gradients in the image space, avoids the computational complexity of Hough transformation, constructs the support domain of the straight line segment by region growth from the seed points, obtains the final line segment region by refining the original region and the rectangular region, has good directional consistency in the line segment region extracted by the LSD algorithm, and greatly improves the precision and the instantaneity compared with Hough transformation.
Parameterized representations of three-dimensional line segments fall into two main categories: nonlinear four-parameter minimization representation and linear over-parameter representation. The dimension of the nonlinear minimum representation method such as orthonormal representation is equal to the degree of freedom of the three-dimensional line segment, so that the nonlinear minimum representation method is used for nonlinear optimization, and cannot induce observability problems, but is difficult to directly represent geometric transformation of the nonlinear minimum representation method; the over-parametric representation method such as Plucker can then be used better for the transformation. On the reconstruction of three-dimensional line segments, there is reported a Cayley representation method under a Plucker coordinate system, which can decouple parameters to optimize a bundle while meeting the Plucker constraint, but faces non-trivial line segment matching and translational motion drift, and has singularities. To solve the problem of end point instability, solutions have been reported in the prior art that decouple translation and rotation and relax constraints, but the assumption that two parallel lines are orthogonal to the third line limits their practical application. Therefore, in the present invention, orthogonal representation is used only inside the estimator for three-dimensional straight lines, and Plucker representation is used in the European transformation, triangularization, projective transformation and other modules of three-dimensional straight lines.
On an odometer and SLAM system for fusing line features, the line features are widely existing in an artificial environment, researchers can divide the fused line features in the SLAM system into two ideas, one type is based on the prior that the line features in the artificial environment have parallel, vertical, coplanar and other relations, the line features are used as extra geometric constraints to improve the performance of the estimator, and the other type is used as a visual information source parallel to the point features. The former focuses on the representation of geometric constraints, and not on the tracking and reconstruction of the line features themselves, which typically requires implementing a complete feature generation and management module at the front-end and back-end, and selecting the appropriate line feature representation.
Although the visual inertial SLAM architecture has matured, the visual inertial system with the fused line features has not yet formed a stable solution, and for the visual inertial system with the line features as parallel visual information sources, the following problems are still needed to be solved:
1) LSD and LBD are dominant streamline feature extraction and matching methods applied to SLAM systems at present, the algorithm is designed for a general visual task, the real-time performance of the SLAM system cannot be considered, for example, the frame rate of PL VIO at a desktop end is lower than 10fps, and compared with 30fps of VINS-Mono, the method is far more difficult to practically apply. And under the condition that the observer continuously moves, the line characteristics are extremely easy to be shielded and disconnected, and the LSD and LBD algorithms cannot process the mismatching phenomenon.
2) The line features do not have a natural compact representation, and an improper parameterization method leads to performance degradation of the estimator due to observability, and an optimization scheme based on endpoint construction error terms does not actually utilize structural information of the line features, and an appropriate parameterization scheme or an indirect representation is introduced for representing and optimizing the line features.
3) The fusion of the dotted line features is still in a mainstream mode by filtering, and the nonlinear optimization method requires maintenance of a reasonable line feature management module, otherwise, the addition of the line features may cause degradation of system performance and degradation of estimator performance.
Against the background, the invention realizes a visual inertial odometer which can run in real time and with high precision and fuses line characteristics based on a VINS-Mono framework and a line characteristic tracking algorithm based on an LSD algorithm. The test result of the invention on the EuRoC data set shows that compared with the original synchronous positioning and map construction system based on the characteristic points, the positioning precision is obviously improved, and the line characteristic map construction achieves better effect.
Disclosure of Invention
Aiming at the defects of the prior art, the invention realizes the visual inertial odometer which can run in real time and with high precision, and compared with the prior synchronous positioning and map construction system based on characteristic points, the positioning precision is obviously improved, and the line characteristic map construction achieves better effect.
In the visual inertia initialization process, the present invention uses conventional line feature extraction and matching algorithms to generate an initial map of line features. In the online feature management module, the invention utilizes the front and back frame motion information of IMU short-time integral structure to induce the process of extraction and matching of the online features, greatly reduces the time consumption of matching the online features, realizes better matching robustness, and simultaneously provides a plurality of strategies of generating and deleting the online features at the back end according to the effect. Finally, the invention realizes an estimator with the tight coupling of the point line characteristics and the inertial navigation information based on the sliding window and the nonlinear optimization theory.
In order to achieve the above purpose, the present invention adopts the following technical scheme:
a visual inertial odometer method of fusing dotted line features, comprising the steps of:
step 1: initialization phase
According to the original 2D image information and IMU measured values, a motion recovery structure routine is utilized to fully converge pose and map coordinate information; then compensating the precision of inertial navigation by utilizing the precision of a vision system, estimating the bias and gravity vector of the inertial navigation, and correcting the inertial navigation parameters; finally, a small-scale 3D map is obtained;
step 2: optical flow tracking stage
Tracking feature points in the pixels by using KLT optical flow, and transmitting the matched point features to the back end; in optical flow tracking, predicting a projection point of a feature point corresponding to a previous frame on a current frame according to an IMU short-time integral result between two frames, and taking the projection point as an initial value to participate in optical flow calculation;
step 3: line feature tracking stage
Tracking the line characteristics by utilizing a line characteristic tracking algorithm, wherein the line characteristic tracking algorithm comprises a line segment prediction stage based on front and rear frame motion information and a line segment detection stage based on a prediction result, and transmitting the matched line characteristics to the rear end; considering the accumulated error introduced by the tracking algorithm, extracting and matching the line characteristic again after the degradation of the line characteristic tracking or a certain time of tracking, and continuing tracking by utilizing the new line characteristic;
step 4: backend estimation stage
Providing the IMU short-time integral between two frames to an optical flow tracking stage and a line characteristic tracking stage at the front end; after receiving the matching point feature pair and the matching line feature pair from the front end tracking, fusing visual information and IMU information, solving a five-element vector group of a system motion state and a three-dimensional coordinate of visual observation, and updating an initialized 3D map;
when a new key frame is added, the estimator performs cluster optimization, so that the accumulated error of the system is reduced; after the key frames in the sliding window reach the designated number, the system will implement marginalization, control the maximum number of states maintained, and thus maintain the computing instantaneity.
Further, in step 1, line characteristic information is added in the motion restoration structure routine, and the correction process is accelerated by using line characteristics in the gravity vector estimation process.
Further, in step 3, a real-time line feature tracking algorithm based on an LSD algorithm is adopted, first, line feature coordinates of a current frame are predicted based on motion information between a previous frame and a next frame, then, image pixel information near the predicted coordinates is used for updating a prediction result, and a final line feature extraction and matching result is generated.
Further, the step 3 specifically includes:
3.1: before tracking starts, firstly triangulating line features in a scene; after tracking starts, pose information of a current frame is obtained through IMU short-time integration between two frames;
segment projection prediction: transforming the triangulated three-dimensional line characteristic to the current frame to obtain corresponding coordinates according to the predicted pose result, calculating the two-dimensional line characteristic projection of the three-dimensional linear characteristic on the current frame, and screening the two-dimensional line characteristic according to the ratio of the three-dimensional line characteristic length to the two-dimensional line characteristic length to obtain a predicted line segment;
sample point optical flow tracking: uniformly sampling on a line segment of a previous frame and a line segment of the projection prediction of a current frame to obtain a plurality of sampling points, projecting the sampling points to the current frame, tracking the two groups of sampling points through optical flow tracking, and correcting the sampling points on the predicted line segment;
3.2: expanding a line segment supporting domain and generating a straight line segment by taking the predicted sampling points as the center, and detecting the direction consistency between the line segment supporting domain and the predicted line segment for a plurality of line segments corresponding to all the sampling points;
3.3: and fusing the direction and the length of the line segment with good consistency with the direction of the predicted line segment to obtain a line segment detection result finally.
Further, expanding the two-dimensional line segment corresponding to the line segment supporting domain requires detecting the included angle between the two-dimensional line segment supporting domain and the predicted line segment, and discarding the two-dimensional line segment with the included angle larger than the threshold value.
Further, a joint loss function is constructed according to the reprojection errors of the point features and the line features and the IMU pre-integration error term, and the close coupling fusion of the point features, the line features and the inertial navigation information is realized through a sliding window.
Further, for the marginalization strategy, when a certain frame arrives, the latest frame in the sliding window is associated with the frame, and then the marginalized object is determined according to whether the next new frame in the sliding window is a key frame or not; when the next new frame in the sliding window is a key frame, the earliest frame in the edge sliding window is formed; when the next new frame in the sliding window is not a key frame, the next new frame will be discarded, but the IMU pre-integral term for that frame is retained.
Compared with the prior art, the invention has the advantages that:
the invention builds a visual inertial synchronous positioning and map construction system fusing line characteristics based on a VINS-Mono framework and a line characteristic tracking algorithm based on an LSD algorithm. The fusion line feature still faces several problems in existing SLAM systems: insufficient tracking real-time, difficulty in correctly processing occlusion and local blurring, and lack of a complete mapping module in processing of online features. The real-time line characteristic tracking method designed in the invention greatly reduces the time consumption of line characteristic matching and realizes better matching robustness.
Compared with the original synchronous positioning and map construction system based on the feature points, the positioning precision of the invention is obviously improved, the line feature map construction achieves better effect, and the invention constructs a complete system based on the VINS-Mono, can be conveniently transplanted to mobile equipment, and is more suitable for scenes such as augmented reality, mobile robots and the like.
Drawings
FIG. 1 is a schematic view of the overall framework of the present invention;
FIG. 2 is a key frame decision and feature tracking correlation flow chart of the present invention;
FIG. 3 is a factor graph in the back-end optimizer of the present invention;
fig. 4 is a schematic diagram of the mh_05_diffull sequence of the present invention, wherein fig. 4-1 is a top view and fig. 4-2 is a front view.
Detailed Description
The present invention will be described in detail with reference to the accompanying drawings. The technical features of the embodiments of the invention can be combined correspondingly on the premise of no mutual conflict.
The invention discloses a visual inertial odometer method integrating dotted line characteristics. Based on VINS-Mono, the invention adds a line feature tracking module at the front end and realizes a line feature management module completely parallel to the point features at the rear end. The added modules greatly reduce the tracking time consumption of the line features and effectively control the map scale. In addition, the invention constructs a joint loss function based on the re-projection error of the point line characteristic and the pre-integral error term of the inertial navigation information, and completes the tight coupling fusion of the point line characteristic and the inertial navigation information through sliding window optimization, so as to avoid the performance degradation of the estimator caused by over-parameterization.
The overall framework of the visual inertial odometer system incorporating the dotted line features is shown in fig. 1, and mainly comprises the following stages:
1.1: and (3) initializing. The stage utilizes original 2D image information and IMU measurement values, and fully converges pose and map coordinate information by running a small motion restoration structure (Structure from Motion, sfM); and then compensating the precision of inertial navigation by utilizing the precision of a vision system, estimating important internal states such as bias, gravity vector and the like of the inertial navigation, and finally obtaining a small-scale 3D map for subsequent use of the system.
1.2: front-end optical flow and line feature tracking stage. The KLT optical flow is used to track feature points in the pixels and the matched point features are transmitted to the back end for further processing. The improved line characteristic tracking algorithm is used for tracking the line characteristics, and the line characteristic tracking method comprises a line segment prediction stage based on the motion information of the front frame and the back frame and a line segment detection stage based on a prediction result, and the matched line characteristics are transmitted to the back end for further processing.
Considering the accumulated error introduced by the tracking algorithm, after the online characteristic tracking is degenerated or tracked for a certain time, the re-extracted and matched line characteristic is provided for a back-end estimator thread, so that a new line characteristic is generated in the system, and the tracking is ensured to continue steadily.
1.3: and a back-end estimation stage. The device comprises three modules: the system comprises a point feature management module, a line feature management module and a sliding window optimization module. The same as the VINS-Mono, the IMU pre-integration between two frames is completed in the stage of optical flow and line characteristic tracking at the front end;
after receiving the matching point feature pair and the matching line feature pair from the front end tracking, the visual information and the IMU information are further fused, and a five-membered vector group of the motion state of the system and the three-dimensional coordinates of the visual observation are solved. When a new key frame is added, the estimator performs cluster optimization, so that the accumulated error of the system is reduced; after the key frames in the sliding window reach the designated number, the system will implement marginalization, control the maximum number of states maintained, and thus maintain the computing instantaneity.
The improvement of the VINS-Mono initialization stage specifically comprises the following steps:
the VINS-Mono runs a small motion restoration structure routine during initialization to fully converge pose and map coordinate information, and then uses the precision of the vision system to compensate the precision of inertial navigation, estimate the bias of inertial navigation, gravity vector and other important internal states for correction. The invention adds line characteristic information in the motion recovery structure routine, and uses the line characteristic in the gravity vector estimation, thereby accelerating the correction process of the gravity vector and improving the precision of the initialization process.
The invention discloses a real-time line characteristic tracking algorithm designed based on an LSD algorithm, which comprises the following specific steps:
the line characteristic tracking algorithm provided by the invention is divided into two steps of prediction and updating. In the prediction stage, the system predicts the line feature coordinates of the current frame based on the motion information between the previous frame and the next frame, and in the updating stage, the system updates the prediction result according to the image pixel information near the prediction coordinates to generate final line feature extraction and matching results.
Specifically, the prediction stage comprises three steps of pre-integration pose prediction, line segment projection prediction and sampling point light flow tracking.
Firstly, for IMU measurement data between a j frame and a j+1 frame, calculating a pre-integration value between the two frames, and predicting the pose of the j+1 frame according to the pre-integration result. Due to noise and calibration errors, there is typically a large error between the IMU integrated pose and the true pose. In the visual inertial system, however, the back-end optimizer will feed back relatively accurate IMU on-line calibration results of the internal and external parameters to the front-end in real time. Therefore, the short-time integration of the existing measurement data by using the IMU parameters optimized by the estimator can achieve higher precision. And then, for line segment projection prediction, according to the pose of the j+1 frame, transforming the triangulated three-dimensional line feature into the j+1 frame to obtain corresponding coordinates, and calculating the two-dimensional line feature projection of the three-dimensional line feature in the j+1 frame. The invention calculates the ratio of the observed three-dimensional line characteristic length to the two-dimensional line characteristic lengthWherein the method comprises the steps ofIs L l Corresponding two-dimensional line feature endpoints, < >>Is L l Corresponding three-dimensional line feature end points, if the two-dimensional line feature length is smaller than the threshold t=20 or +.>The line segment will be discarded.
And finally, uniformly sampling on the line segment of the previous frame and the line segment of the projection prediction of the next frame to respectively obtain a plurality of corresponding sampling points. And tracking the two groups of sampling points by using the KLT optical flow, and correcting the sampling points on the predicted line segment. And obtaining a corrected predicted line segment on the corrected sampling point set by a least square method.
The updating stage comprises two steps of line segment detection and fusion and three-dimensional line coordinate updating.
Firstly, in the steps of segment detection and fusion, expanding a segment support domain by taking the corrected sampling point as a center, and completing the processes of region correction and the like to obtain a segment detection result corresponding to the sampling point. And detecting the direction consistency between the multiple line segments returned by all the sampling points and the predicted line segments obtained in the prediction stage, and carrying out line segment fusion on line segments with similar directions.
Secondly, after the pose of the current frame is updated by the rear end, the system re-triangulates the matched line features by using the updated pose to obtain three-dimensional line feature coordinates in the j+1 frame, and updates the three-dimensional line feature coordinates by simple averaging. For line features that the current frame did not track successfully, the coordinate update process is skipped. The line segment successfully detected in the step has smaller two-dimensional observation change amplitude before and after updating, and the line segment with larger observation error is removed, so that the three-dimensional line characteristic updating process is more stable, and abnormal values can not occur.
The line segment fusion related to the updating stage of the line characteristic tracking algorithm comprises the following specific steps:
for three-dimensional line features, use is made ofRepresenting the first line characteristic at C j Plucker coordinates in the frame coordinate system; use->Representing the first line characteristic at C j Endpoint coordinates in the frame coordinate system. For two-dimensional line features +.>Representing a two-dimensional observation of the ith line feature in the jth frame of image. />Direction vector with length defined as two-dimensional line feature,/->Defined as the center point of the two-dimensional line feature.
For the two-dimensional line segment corresponding to the supporting domain, detecting an included angle between the two-dimensional line segment and the direction of the predicted line segment, and discarding if the included angle is larger than pi/10; for the rest line segments, since the line segments with larger lengths generally have more accurate line segment detection results, we solve the weighted average direction of the line segments by taking the length of the line segments as the weightAt->Calculating projections of line segment end points in the direction, and calculating maximum distance between projection points +.>Two endpoints corresponding to the maximum distance +.>Then there are:the corresponding two-dimensional line segment is the final two-dimensional line segment detection result. If the process is stopped, the tracking is failed.
The key frame decision and feature tracking related flow chart related to the back-end line feature management module is shown in fig. 2. The key frame decision, the line feature tracking and the line feature removing are specifically as follows:
when deciding the key frame, the invention decides the key frame according to three standards of the feature point tracking quantity, the parallax and the interval frame number. Firstly, the system detects the number of successfully tracked feature points, and when the number of the successfully tracked feature points is smaller than 50, the current frame is directly used as a key frame. Secondly, if the tracking quantity is sufficient, the system projects the characteristic points of the previous frame to the current frame by using the rotation result of IMU short-time pre-integration to calculate parallax, and if more than 4/5 of successfully tracked characteristic points have more than 50 pixels of parallax, the current frame is marked as a key frame. Finally, if the previous frame is more than 10 frames from the previous frame, the current frame will also be marked as a key frame.
In the step of on-line feature tracking, similar to the quality judgment of optical flow tracking, the invention also determines the tracking quality of the line features according to the matching quantity and the interval frame number of the line feature tracking. When the quality of the line feature tracking is degraded (the number of matches is less than 10) or after a certain number of frames (20 frames) pass, the system will rerun the LSD algorithm on the unused area of the current frame and the next frame and triangulate the new line feature, thereby ensuring the number and quality of the line feature.
When the line features are removed, due to the fact that certain spatial locality exists in the line features, part of the line features of the three-dimensional map are difficult to observe in the follow-up tracking process, the tracking quality of part of the line features is poor, and necessary screening can be conducted according to the line feature tracking process. The present invention removes line features with two basic criteria: the line features are not successfully tracked for more than 5 frames in 10 frames in the history frames; the line feature calculates an observed length ratio of less than 0.1 for 5 consecutive frames in the history frame. Since the detection criteria are set within a certain historical time, line features that are not utilized due to occlusion and short-term angle changes remain in the map points and are not repeatedly triangulated.
The back-end sliding window optimization involves an marginalization strategy and a joint loss function, specifically:
the marginalization strategy is as shown in fig. 3, when a certain frame arrives, the latest frame in the window has a relatively sufficient association with the frame, and must be reserved, and the marginalized object is determined according to whether the next new frame in the sliding window is a key frame or not. When the next new frame in the sliding window is a key frame, the earliest frame in the marginal sliding window and the observation relation thereof are used for avoiding information loss in the system. When the next new frame in the sliding window is not a key frame, the next new frame is discarded, but the IMU pre-integral item of the frame is reserved, so that IMU information corresponding to the next new frame resides in the sliding window to be fully optimized. After marginalizing the corresponding state, a priori information items are added to the error function to participate in the subsequent optimization of the sliding window.
For the joint loss function, first define the system state:
wherein x is i For the set of motion state five-membered vectors in the estimator, lambda m As a feature of a three-dimensional point,for three-dimensional line features using orthogonal representation in the estimator +.>Rotation and translation of the IMU coordinate system to the world coordinate system, respectively.
The joint error function of the sliding window designed by the invention is as follows:
wherein the method comprises the steps ofFor the pre-integral residual term, ρ (·) is a robust kernel function, ++>Pre-integration representing IMU observations +.>Motion state in the AND estimator>Residual between->Is a classical point feature reprojection error, wherein +.>Is f k The corresponding feature is c i Two-dimensional observations on frame images.
Is a line feature->At c i Reprojection errors under the frame. The error term is characterized by three-dimensional lines +.>Projection on the camera plane +.>And corresponding two-dimensional observation line segment->The point-line distance d (s, 1) of the end point:
wherein:
the invention uses Ceres structural factor graph as a solving tool, the solving method uses a column Wen Boge-Marquardt algorithm, and the robust kernel function of the error term uses Huber function.
Examples
To further demonstrate the effect of the present invention, the present example uses the EuRoC dataset for evaluation. The dataset contains 11 motion sequences covering different motion and environmental modes of fast motion, fast rotation, poor lighting conditions, motion blur, etc.
Evaluation index:
run time: the system designed by the invention is used for averaging over EuRoC data sets with VINS-Mono and PL VIO.
Positioning accuracy: the positioning effect is evaluated in detail by using EVO, and APE (Absolute Positional Error, absolute position error) is adopted as an evaluation index.
Experiment 1: the present invention and the average time spent on the vies-Mono, PL VIO on the EuRoC dataset this example records the average time spent on the feature tracking, estimator (PnP, bundling adjustment, marginalization, etc.), pre-integration, frame averaging module, and the total system time spent, respectively, for the three systems as shown in table 1. It can be seen that the system of the present invention can be run in real time on the data set at a rate of 20 frames per second.
Table 1: average time-consuming on EuRoC dataset
Experiment 2: positioning accuracy test
In the embodiment, the positioning effect is evaluated in detail by using the EVO, and the evaluation index adopts APE, and because the visual inertial odometer designed by the text does not contain a closed-loop module, the VINS Mono data used as comparison is a positioning result without the closed-loop module.
The results of the evaluation on the EuRoC dataset are shown in table 2, with bolded fonts indicating better results. In most sequences, the system designed by the invention is overall better than the positioning precision of the VINS Mono, and particularly has obvious advantages relative to the VINS for sequences (MH_04_difficut, MH_05_difficut and V2_02_medium) with illumination change and rapid rotation.
Table 2: APE (m) of EuRoC dataset
| Ours | VINS-Mono | PL VIO | |
| MH_01_easy | 0.1384 | 0.1547 | 0.1403 |
| MH_02_easy | 0.1448 | 0.1784 | 0.1429 |
| MH_03_medium | 0.1723 | 0.1988 | 0.2761 |
| MH_04_difficult | 0.3544 | 0.4399 | 0.3825 |
| MH_05_difficult | 0.2869 | 0.3008 | 0.2908 |
| V1_01_easy | 0.0807 | 0.0889 | 0.0793 |
| V1_02_medium | 0.0913 | 0.1126 | 0.1046 |
| V2_01_easy | 0.0885 | 0.0823 | 0.0927 |
| V2_02_medium | 0.1425 | 0.1620 | 0.1488 |
| V2_03_difficult | 0.2756 | 0.2870 | 0.2894 |
Experiment 3: drawing effect
In this embodiment, fig. 4 shows three-dimensional line features, point features and trajectories on mh_05_difficult sequence, where fig. 4-1 is a top view and fig. 4-2 is a front view.
Firstly, the vertical and horizontal structural information in the scene is quite rich, and the parallel and vertical relations between the line features conform to the structure in the original scene, so that the line features can better reveal the scene structure compared with discrete point features.
Secondly, the recognition effect of the line features is good, the detection results of most line segments in the scene show identity and are not recognized as a plurality of different line features, the effect is benefited by a merging and rejecting module of the line features, the line features with shorter life time or redundancy in the scene are not reserved in the map, and the structure of the map is clear.
Finally, a good parallel relation is presented between the vertical line feature in the scene and the vertical direction of the coordinate axis and between the horizontal directions of the line feature, which indicates that after the visual inertia joint initialization, the calibration of the gravity vector direction is accurate, so that the recovery of the z direction of the vertical line feature is accurate.
From the comparative experiments the following conclusions can be drawn:
compared with the existing SLAM systems such as VINS-Mono and PL VIO, the positioning accuracy of the invention is obviously improved, and the line characteristic mapping achieves better effect.
The foregoing list is only illustrative of specific embodiments of the invention. Obviously, the invention is not limited to the above embodiments, but many variations are possible. All modifications directly derived or suggested to one skilled in the art from the present disclosure should be considered as being within the scope of the present invention.
Claims (4)
1. A visual inertial odometer method incorporating dotted line features, comprising the steps of:
step 1: initialization phase
According to the original 2D image information and IMU measured values, a motion recovery structure routine is utilized to fully converge pose and map coordinate information; then compensating the precision of inertial navigation by utilizing the precision of a vision system, estimating the bias and gravity vector of the inertial navigation, and correcting the inertial navigation parameters; finally, a 3D map is obtained;
step 2: optical flow tracking stage
Tracking feature points in the pixels by using KLT optical flow, and transmitting the matched point features to the back end; in optical flow tracking, predicting a projection point of a feature point corresponding to a previous frame on a current frame according to an IMU short-time integral result between two frames, and taking the projection point as an initial value to participate in optical flow calculation;
step 3: line feature tracking stage
Tracking the line characteristics by utilizing a line characteristic tracking algorithm, wherein the line characteristic tracking algorithm comprises a line segment prediction stage based on front and rear frame motion information and a line segment detection stage based on a prediction result, and transmitting the matched line characteristics to the rear end; considering the accumulated error introduced by the tracking algorithm, extracting and matching the line characteristic again after the degradation of the line characteristic tracking or a certain time of tracking, and continuing tracking by utilizing the new line characteristic;
the step 3 specifically comprises the following steps:
3.1: before tracking starts, firstly triangulating line features in a scene; after tracking starts, pose information of a current frame is obtained through IMU short-time integration between two frames;
segment projection prediction: transforming the triangulated three-dimensional line characteristic to the current frame to obtain corresponding coordinates according to the predicted pose result, calculating the two-dimensional line characteristic projection of the three-dimensional linear characteristic on the current frame, and screening the two-dimensional line characteristic according to the ratio of the three-dimensional line characteristic length to the two-dimensional line characteristic length to obtain a predicted line segment;
sample point optical flow tracking: uniformly sampling on a line segment of a previous frame and a line segment of the projection prediction of a current frame to obtain a plurality of sampling points, projecting the sampling points to the current frame, tracking the two groups of sampling points through optical flow tracking, and correcting the sampling points on the predicted line segment;
3.2: expanding a line segment supporting domain and generating a straight line segment by taking the predicted sampling points as the center, and detecting the direction consistency between the line segment supporting domain and the predicted line segment for a plurality of line segments corresponding to all the sampling points;
3.3: for the line segment with the same direction as the predicted line segment, fusing the direction and the length of the line segment to obtain a line segment detection result finally;
step 4: backend estimation stage
Providing the IMU short-time integral between two frames to an optical flow tracking stage and a line characteristic tracking stage at the front end; after receiving the matching point feature pair and the matching line feature pair from the front end tracking, fusing visual information and IMU information, solving a five-element vector group of a system motion state and a three-dimensional coordinate of visual observation, and updating an initialized 3D map;
when a new key frame is added, the estimator performs cluster optimization, so that the accumulated error of the system is reduced; after the key frames in the sliding window reach the designated number, the system performs marginalization, and controls the maximum state number of maintenance, so that the calculation instantaneity is maintained; for the marginalization strategy, when a certain frame arrives, the latest frame in the sliding window is associated with the frame, and then the marginalized object is determined according to whether the next new frame in the sliding window is a key frame or not; when the next new frame in the sliding window is a key frame, the earliest frame in the edge sliding window is formed; when the next new frame in the sliding window is not a key frame, the next new frame will be discarded, but the IMU pre-integral term for that frame is retained;
and constructing a joint loss function according to the reprojection errors of the point features and the line features and the IMU pre-integration error term, and realizing the tight coupling fusion of the point features, the line features and the inertial navigation information through a sliding window.
2. The visual odometer method of claim 1, wherein the line feature information is added to the motion restoration structure routine in step 1, and the line feature is used in the gravity vector estimation to accelerate the correction process.
3. The visual inertial odometer method according to claim 1, wherein in step 3, a real-time line feature tracking algorithm based on LSD algorithm is adopted, line feature coordinates of a current frame are predicted based on motion information between a previous frame and a next frame, and then image pixel information near the predicted coordinates is used to update the prediction result, so as to generate final line feature extraction and matching results.
4. The visual inertial odometer method according to claim 1, wherein expanding the two-dimensional line segment corresponding to the line segment supporting domain requires detecting an angle between the two-dimensional line segment and the predicted line segment, and discarding the two-dimensional line segment with the angle greater than the threshold value.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110727116.5A CN113532420B (en) | 2021-06-29 | 2021-06-29 | Visual inertial odometer method integrating dotted line characteristics |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110727116.5A CN113532420B (en) | 2021-06-29 | 2021-06-29 | Visual inertial odometer method integrating dotted line characteristics |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113532420A CN113532420A (en) | 2021-10-22 |
| CN113532420B true CN113532420B (en) | 2024-01-30 |
Family
ID=78126190
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110727116.5A Active CN113532420B (en) | 2021-06-29 | 2021-06-29 | Visual inertial odometer method integrating dotted line characteristics |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113532420B (en) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114001733B (en) * | 2021-10-28 | 2024-03-15 | 浙江大学 | Map-based consistent efficient visual inertial positioning algorithm |
| CN116188536B (en) * | 2023-04-23 | 2023-07-18 | 深圳时识科技有限公司 | Visual inertial odometer method and device and electronic equipment |
| CN116630901B (en) * | 2023-07-24 | 2023-09-22 | 南京师范大学 | Visual odometer method based on potential diagram prediction non-supervision learning framework |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101216950A (en) * | 2008-01-16 | 2008-07-09 | 浙江大学 | An elastic deformation simulation method based on linear differential operator |
| US8756001B2 (en) * | 2011-02-28 | 2014-06-17 | Trusted Positioning Inc. | Method and apparatus for improved navigation of a moving platform |
| CN109859266A (en) * | 2019-01-28 | 2019-06-07 | 西安理工大学 | Vision positions and drawing practice simultaneously under a kind of big visual angle change based on pre-transform |
| CN111024077A (en) * | 2019-12-30 | 2020-04-17 | 北京航空航天大学 | All-optical bionic autonomous navigation system in complex environment |
| CN112649016A (en) * | 2020-12-09 | 2021-04-13 | 南昌大学 | Visual inertial odometer method based on point-line initialization |
-
2021
- 2021-06-29 CN CN202110727116.5A patent/CN113532420B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101216950A (en) * | 2008-01-16 | 2008-07-09 | 浙江大学 | An elastic deformation simulation method based on linear differential operator |
| US8756001B2 (en) * | 2011-02-28 | 2014-06-17 | Trusted Positioning Inc. | Method and apparatus for improved navigation of a moving platform |
| CN109859266A (en) * | 2019-01-28 | 2019-06-07 | 西安理工大学 | Vision positions and drawing practice simultaneously under a kind of big visual angle change based on pre-transform |
| CN111024077A (en) * | 2019-12-30 | 2020-04-17 | 北京航空航天大学 | All-optical bionic autonomous navigation system in complex environment |
| CN112649016A (en) * | 2020-12-09 | 2021-04-13 | 南昌大学 | Visual inertial odometer method based on point-line initialization |
Non-Patent Citations (1)
| Title |
|---|
| 鲁棒单目视觉惯性里程计初始化方法;黄仁强，等;工业控制计算机;第32卷(第1期);70-73 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113532420A (en) | 2021-10-22 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113532420B (en) | Visual inertial odometer method integrating dotted line characteristics | |
| CN110223348B (en) | Robot scene self-adaptive pose estimation method based on RGB-D camera | |
| CN109166149B (en) | Positioning and three-dimensional line frame structure reconstruction method and system integrating binocular camera and IMU | |
| US10762645B2 (en) | Stereo visual odometry method based on image gradient joint optimization | |
| WO2018129715A1 (en) | Simultaneous positioning and dense three-dimensional reconstruction method | |
| CN108615246B (en) | Method for improving robustness of visual odometer system and reducing calculation consumption of algorithm | |
| CN112304307A (en) | Positioning method and device based on multi-sensor fusion and storage medium | |
| CN101563709B (en) | Calibrating a camera system | |
| CN108648215B (en) | SLAM motion blur pose tracking algorithm based on IMU | |
| CN112902953A (en) | Autonomous pose measurement method based on SLAM technology | |
| Liu et al. | Direct visual odometry for a fisheye-stereo camera | |
| CN108519102B (en) | Binocular vision mileage calculation method based on secondary projection | |
| CN109974743B (en) | Visual odometer based on GMS feature matching and sliding window pose graph optimization | |
| CN106875482A (en) | A kind of positioning simultaneously and dense three-dimensional rebuilding method | |
| CN110726406A (en) | Improved nonlinear optimization monocular inertial navigation SLAM method | |
| CN110322492B (en) | Space three-dimensional point cloud registration method based on global optimization | |
| CN110751123B (en) | Monocular vision inertial odometer system and method | |
| CN111882602A (en) | Visual odometer implementation method based on ORB feature points and GMS matching filter | |
| US20230298344A1 (en) | Method and device for determining an environment map by a server using motion and orientation data | |
| CN112767546B (en) | Binocular image-based visual map generation method for mobile robot | |
| CN116342661A (en) | Binocular vision inertial odometer method for correcting pose by using road mark point offset | |
| Zhou et al. | Semi-dense visual odometry for RGB-D cameras using approximate nearest neighbour fields | |
| CN113587904A (en) | Target attitude and position measurement method integrating machine vision and laser reference point information | |
| Lebegue et al. | Extraction and interpretation of semantically significant line segments for a mobile robot | |
| CN117115271A (en) | Binocular camera external parameter self-calibration method and system in unmanned aerial vehicle flight process |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |