Visual SLAM method and equipment based on Manhattan world structure space fusion

Abstract

The present invention discloses a visual SLAM method and device based on Manhattan world structure spatial fusion. The method comprises: acquiring environmental data from a visual sensor, performing structured line feature extraction and spatial cognitive information extraction to obtain the three main direction vanishing points of the indoor space; correcting the initial values ​​of the triangulation of the structured line features using spatial cognitive information fusion; constructing a structured line feature error optimization equation, using the corrected line coordinates as the initial values ​​for nonlinear optimization, and obtaining SLAM mapping and positioning information. The present invention targets indoor scenes that conform to the Manhattan world structure, and by performing structured line feature extraction and spatial cognitive information extraction, and by using spatial cognitive information fusion to correct the structured line features and participate in error constraint and optimization, thereby improving positioning accuracy and the reliability of 3D structured feature maps.

Description

Technical Field

[0001] The present invention relates to the field of robot perception technology, and in particular to a visual SLAM method and device based on Manhattan world structure space fusion. Background Art

[0002] SLAM stands for Simultaneous Localization and Mapping (SLAM), and is primarily used to solve the positioning and mapping problems of robots moving in unknown environments. SLAM is broadly classified into two types: Lidar SLAM (Lidar SLAM) and Visual SLAM (VSLAM), which is based on vision.

[0003] In recent years, service robots have rapidly gained popularity and developed in indoor environments. The advantage of vision-based SLAM technology lies in its utilization of rich texture information. For example, a point cloud-based laser SLAM algorithm cannot distinguish between two billboards of the same size but with different contents, while vision can easily distinguish them. This brings unparalleled advantages in relocalization and scene classification. Furthermore, visual information can be easily used to track and predict dynamic objects in a scene, such as pedestrians and vehicles, which is crucial for applications in complex and dynamic scenes.

[0004] However, existing vision-based SLAM technologies mostly use point features in images to estimate 3D poses, but they are prone to accuracy degradation and tracking failure in indoor low-texture scenes. Furthermore, due to a lack of spatial cognition, the constructed maps are inaccurate, leading to errors in the robot's trajectory. Summary of the Invention

[0005] The purpose of the present invention is to overcome the shortcomings of the existing technology and provide a visual SLAM method and device based on Manhattan world structure spatial fusion, which fuses the Manhattan world structure and spatial perception information to enable visual SLAM to improve the accuracy of precision, tracking, and map information in indoor low-texture scenes, thereby ensuring the rationalization of the robot's trajectory.

[0006] First, the technical solution of the present invention is achieved as follows:

[0007] Visual SLAM method based on Manhattan world structure space fusion, including:

[0008] Acquire environmental data from visual sensors, perform structured line feature extraction and spatial cognitive information extraction, and obtain the three main direction vanishing points of the indoor space;

[0009] The initial value of triangulation of structured line features is modified by using spatial cognitive information fusion;

[0010] The structured straight line feature error optimization equation is constructed, and the corrected straight line coordinates are used as the initial value of nonlinear optimization to obtain SLAM mapping and positioning information.

[0011] As a further optimization, the extracted structured straight line features at least include straight line features extracted based on the structures of the walls, floors and ceilings of the indoor space that conform to the Manhattan world structure.

[0012] As a further optimization, the visual SLAM method based on Manhattan world structure space fusion is characterized in that the method of extracting the spatial cognitive information includes: introducing the posture data of the inertial sensor to calculate the vanishing point in the gravity direction; constructing a unit sphere with the camera as the center, and sampling on the unit sphere to generate a vanishing point hypothesis in the camera coordinate system, and verifying the vanishing point hypothesis according to the orthogonal relationship of the vanishing point and the line segment response to obtain the optimal vanishing point in the three main directions.

[0013] As a further optimization, the spatial cognition information extraction method introduces the posture data of the inertial sensor to calculate the vanishing point in the gravity direction to obtain the optimal vanishing points in the three main directions in the camera coordinate system. The specific method includes:

[0014] Define the vanishing point in the camera coordinate system as

[0015] The rotation matrix of the IMU coordinate system at the current moment relative to the first frame is Rs;

[0016] The rotation matrix from the IMU coordinate system to the camera is R cb ;

[0017] According to the transformation relationship between coordinate systems, the vanishing point in the world coordinate system is In the camera coordinate system it is expressed as:

[0018]

[0019] As a further optimization, in the spatial cognitive information extraction method, a unit sphere is constructed with the camera as the center and sampling is performed on the unit sphere to generate a vanishing point hypothesis. The specific method includes: constructing a unit sphere with its X and Y axes parallel to the X and Y axes of the image plane, respectively, and the Z axis pointing to the optical center of the camera. Given the optical center (x0, y0) and focal length f of the camera, the vanishing point pixel coordinates are converted to coordinates on the unit sphere (X, Y, Z), and polar coordinates are established on the unit sphere to obtain the polar coordinate system angles φ and λ:

[0020] in,

[0021] In with The vanishing point hypothesis is generated by uniform sampling on the orthogonal unit sphere plane, with a sampling interval of 1°, that is, a total of 360 vanishing point hypotheses are generated; according to the orthogonal property, the coordinates of the i-th vanishing point hypothesis on the unit sphere are Calculation is done using the following equation:

[0022]

[0023] The third vanishing point hypothesis and and They all satisfy the orthogonal relationship and can be directly calculated based on the vector cross product:

[0024]

[0025] As a further optimization, in the spatial cognitive information extraction method, the method for verifying the vanishing point hypothesis based on the orthogonal relationship of the vanishing points and the line segment response to obtain the optimal vanishing points in the three main directions includes: finding the hypothesis with the largest line segment response among the 360 ​​generated vanishing point hypotheses, using a polar coordinate grid to quickly verify the hypothesis, and the hypothesis with the largest response value is selected as the optimal vanishing point estimate; using the vanishing point sorting in the first frame as a reference, calculating and comparing the Euclidean distance d between the vanishing point coordinates in the current frame and the previous frame, thereby classifying and sorting v2 and v3.

[0026] As a further optimization, the method of spatial cognitive information fusion includes: using vanishing points to classify structured straight line features detected in the image; if the line is not classified by any vanishing point, converting the Plücker coordinate representation obtained by the current triangulation into an orthogonal representation and using it as the initial value for nonlinear optimization of the line parameters; otherwise, the Plücker coordinate representation of the line is corrected using the direction of the corresponding vanishing point.

[0027] As a further optimization, the method for correcting the initial value of the triangulation of the structured line feature is as follows: a line is drawn between the midpoint of a pixel segment of the detected structured line feature and the three vanishing points, and the angle δ between the line and the segment is calculated. If δ is less than a set threshold, the pixel segment is considered to be consistent with the corresponding vanishing point direction and category. Furthermore, if the angle between at least two segments is less than the set threshold, the pixel segment is considered not to belong to any vanishing point direction.

[0028] Assume that the classification results of the straight line and the vanishing point If consistent, As the correction value of the direction vector of the straight line in the world coordinate system;

[0029] After transforming M from the camera coordinate system to the world coordinate system, it is combined with the corrected direction vector d wPerform cross multiplication to update the normal vector n of the line w .

[0030] As a further optimization, the method for constructing the structured straight line feature error optimization equation includes:

[0031] The Plücker coordinate representation of the current structured feature in the world coordinate system L w =(n w d w ), and the corresponding vanishing point direction v=(v x v y v z ), construct the residual term r v as follows:

[0032] The main spatial direction is used to correct the initial value of the structured line feature, and the residual term is constructed to constrain the calculation of SLAM mapping and positioning information.

[0033] In addition, the technical solution of the present invention can also be achieved by:

[0034] Visual SLAM equipment based on Manhattan world structure space fusion, including:

[0035] The spatial information extraction module is used to obtain environmental data from the visual sensor, perform structured line feature extraction and spatial cognitive information extraction, and obtain the three main direction vanishing points of the indoor space;

[0036] The spatial information fusion module is used to modify the initial value of the triangulation of structured line features using spatial cognitive information fusion;

[0037] The error optimization module constructs the structured straight line feature error optimization equation, uses the corrected straight line coordinates as the initial value of nonlinear optimization, and obtains SLAM mapping and positioning information.

[0038] By adopting the above technical solution, the beneficial effects of the present invention are:

[0039] Aiming at indoor scenes that conform to the Manhattan world structure, the present invention performs structured straight line feature extraction and spatial cognitive information extraction, and fuses the spatial cognitive information to correct the structured straight line features and participate in error constraint and optimization, thereby improving positioning accuracy and the reliability of the 3D structured feature map. This is to enhance the accuracy of visual SLAM in indoor low-texture scenes, tracking, and map information, thereby ensuring the rationality of the robot's trajectory. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for describing the embodiments or the prior art.

[0041] Figure 1 This is a flow chart of the visual SLAM method based on Manhattan world structure space fusion of the present invention.

[0042] Figure 2 A schematic diagram of a visual SLAM device based on Manhattan world structure space fusion is given in the present invention. DETAILED DESCRIPTION

[0043] The following is an explanation and description of the technical solutions of the embodiments of the present invention in conjunction with the drawings of the embodiments of the present invention. However, the following embodiments are only preferred embodiments of the present invention and are not exhaustive. Based on the embodiments in the implementation manner, other embodiments obtained by those skilled in the art without making any creative work are all within the scope of protection of the present invention.

[0044] Embodiment 1;

[0045] Figure 1 The present invention shows a visual SLAM method based on Manhattan world structure space fusion, comprising:

[0046] Step S1: Acquire environmental data from the visual sensor, perform structured line feature extraction and spatial cognitive information extraction, and obtain the three main direction vanishing points of the indoor space.

[0047] In this embodiment, the extracted structured straight line features at least include straight line features extracted based on structures such as walls, floors, and ceilings of the indoor space that conform to the Manhattan world structure.

[0048] The spatial cognitive information extraction method includes: introducing the posture data of the inertial sensor to calculate the vanishing point in the gravity direction, constructing a unit sphere with the camera as the center, and sampling on the unit sphere to generate a vanishing point hypothesis in the camera coordinate system. The vanishing point hypothesis is verified based on the orthogonal relationship of the vanishing points and the line segment response to obtain the optimal vanishing points in the three main directions.

[0049] Furthermore, in the spatial cognition information extraction method, the posture data of the inertial sensor is introduced to calculate the vanishing point in the gravity direction to obtain three vanishing points in the camera coordinate system. The specific method includes:

[0050] Define the vanishing point in the camera coordinate system as The rotation matrix of the IMU coordinate system at the current moment relative to the first frame is Rs; the rotation matrix from the IMU coordinate system to the camera is R cb According to the transformation relationship between coordinate systems, the vanishing point in the world coordinate system is In the camera coordinate system it is expressed as:

[0051]

[0052] In the spatial cognitive information extraction method, a unit sphere is constructed with the camera as the center and sampling is performed on the unit sphere to generate vanishing point hypotheses. The specific method includes: constructing a unit sphere with its X-axis and Y-axis parallel to the X-axis and Y-axis of the image plane, respectively, and the Z-axis pointing to the optical center of the camera. Given the optical center (x0, y0) and focal length f of the camera, the vanishing point pixel coordinates are converted to coordinates (X, Y, Z) on the unit sphere, and polar coordinates are established on the unit sphere to obtain the polar coordinate system angles φ and λ:

[0053] in,

[0054] In with The vanishing point hypothesis is generated by uniform sampling on the orthogonal unit sphere plane, with a sampling interval of 1°, that is, a total of 360 vanishing point hypotheses are generated. According to the orthogonal property, the coordinates of the i-th vanishing point hypothesis on the unit sphere are Calculation is done using the following equation:

[0055]

[0056] The third vanishing point hypothesis and and They all satisfy the orthogonal relationship and can be directly calculated based on the vector cross product:

[0057]

[0058] In the spatial cognitive information extraction method, the vanishing point hypotheses are verified based on the orthogonal relationship between vanishing points and line segment responses to obtain the optimal vanishing points in the three principal directions. This involves finding the one with the largest line segment response among the 360 ​​vanishing point hypotheses generated above, rapidly verifying the hypotheses using a polar coordinate grid, and selecting the hypothesis with the largest response as the optimal vanishing point estimate. Using the order of vanishing points in the first frame as a reference, the Euclidean distance d between the vanishing point coordinates in the current and previous frames is calculated and compared to classify and sort v2 and v3.

[0059] Step S2: using spatial cognitive information fusion to correct the initial value of the triangulation of the structured line feature.

[0060] In this embodiment, the spatial cognitive information fusion method includes: using vanishing points to classify structured straight line features detected in the image; if the line is not classified by any vanishing point, converting the Plücker coordinate representation obtained in the current triangulation step into an orthogonal representation and using it as the initial value for nonlinear optimization of the line parameters; otherwise, the Plücker coordinate representation of the line is corrected using the direction of the corresponding vanishing point.

[0061] Specifically, the method for correcting the initial value of the triangulation of the structured line feature is as follows: a line is drawn between the midpoint of the pixel line segment of the detected structured line feature and the three vanishing points, and the angle δ between the line and the line segment is calculated. If δ is less than a set threshold, the line segment is considered to be consistent with the corresponding vanishing point direction and category. It is also stipulated that if the angle between at least two line segments is less than the set threshold, the line segment is considered not to belong to any vanishing point direction.

[0062] Assume that the classification results of the straight line and the vanishing point If consistent, As the correction value of the direction vector of the straight line in the world coordinate system. In order to satisfy the orthogonal constraint of the Plücker coordinate representation, after transforming M from the camera coordinate system to the world coordinate system, it is combined with the corrected direction vector d w Perform cross multiplication to update the normal vector n of the line w .

[0063] Step S3: construct a structured straight line feature error optimization equation, use the corrected straight line coordinates as the initial value of nonlinear optimization, and obtain SLAM mapping and positioning information.

[0064] The Plücker coordinate representation of the current structured feature in the world coordinate system L w =(n w d w ), and the corresponding vanishing point direction v=(v x v y v z ), construct the residual term r v as follows:

[0065]

[0066] The main spatial direction is used to correct the initial value of the structured line feature, and the residual term is constructed to constrain the calculation of SLAM mapping and positioning information to improve the accuracy of the structured feature map constructed by SLAM and retain more semantic information.

[0067] Embodiment 2;

[0068] Figure 2 The present invention shows a visual SLAM device based on Manhattan world structure space fusion, including:

[0069] The spatial information extraction module is used to obtain environmental data from the visual sensor, perform structured line feature extraction and spatial cognitive information extraction, and obtain the three main direction vanishing points of the indoor space;

[0070] The spatial information fusion module is used to modify the initial value of the triangulation of structured line features using spatial cognitive information fusion;

[0071] The error optimization module constructs the structured straight line feature error optimization equation, uses the corrected straight line coordinates as the initial value of nonlinear optimization, and obtains SLAM mapping and positioning information.

[0072] Furthermore, the structured straight line features are straight line features extracted from structures such as walls, floors, and ceilings of indoor spaces that conform to the Manhattan world structure.

[0073] The spatial cognitive information extraction method includes: introducing the posture data of the inertial sensor to calculate the vanishing point in the gravity direction, constructing a unit sphere with the camera as the center and sampling on the unit sphere to generate a vanishing point hypothesis, and verifying the vanishing point hypothesis based on the orthogonal relationship of the vanishing points and the line segment response to obtain the optimal vanishing points in the three main directions.

[0074] The method of introducing the posture data of the inertial sensor to calculate the vanishing point in the gravity direction to obtain three vanishing points in the camera coordinate system includes: defining the vanishing point in the camera coordinate system as The rotation matrix of the IMU at the current moment relative to the first frame is Rs; the rotation matrix from the IMU coordinate system to the camera is R cb According to the transformation relationship between coordinate systems, In the camera coordinate system it is expressed as:

[0075]

[0076] The method of constructing a unit sphere with the camera as the center and sampling on the unit sphere to generate vanishing point hypotheses includes constructing a unit sphere whose X axis and Y axis are parallel to the X axis and Y axis of the image plane respectively, and whose Z axis points to the optical center of the camera. The vanishing point hypothesis is generated by uniform sampling on the orthogonal unit sphere plane. According to the orthogonal property, the coordinates of the i-th vanishing point hypothesis on the unit sphere are Calculation is done using the following equation:

[0077]

[0078] The third vanishing point hypothesis and and They all satisfy the orthogonal relationship and can be directly calculated based on the vector cross product:

[0079]

[0080] The method for verifying vanishing point hypotheses based on the orthogonal relationship between vanishing points and line segment responses to obtain the optimal vanishing points in the three principal directions involves finding the one with the largest line segment response among the 360 ​​vanishing point hypotheses generated above. These hypotheses are then rapidly verified using a polar coordinate grid, with the hypothesis with the largest response value being selected as the optimal vanishing point estimate. Using the order of vanishing points in the first frame as a reference, the Euclidean distance d between the vanishing point coordinates in the current and previous frames is calculated and compared to classify and sort v2 and v3.

[0081] The spatial cognitive information fusion method includes: using vanishing points to classify structured straight line features detected in the image; if the line is not classified by any vanishing point, converting the Plücker coordinate representation obtained in the current triangulation step into an orthogonal representation and using it as the initial value for nonlinear optimization of the line parameters; otherwise, correcting the Plücker coordinate representation of the line using the direction of the corresponding vanishing point.

[0082] The structured line feature classification method includes drawing a line between the midpoint of a pixel line segment of a detected structured line feature and three vanishing points, calculating the angle δ between the line and the line segment, and if δ is less than a set threshold, the line segment is considered to be consistent with the corresponding vanishing point direction and category. Furthermore, if the angle between at least two line segments is less than the set threshold, the line segment is considered not to belong to any vanishing point direction.

[0083] The Plücker coordinate representation of the current structured feature in the world coordinate system L w =(n w d w ), and the corresponding vanishing point direction v=(v x v y v z ), the residual term is constructed as follows:

[0084]

[0085] The main spatial direction is used to correct the initial value of the structured line feature, and the residual term is constructed to constrain the calculation of SLAM mapping and positioning information to improve the accuracy of the structured feature map constructed by SLAM and retain more semantic information.

[0086] It should be noted that the modules (or units) in this embodiment are logical in nature. In specific implementation, multiple modules (or units) can be combined into one module (or unit), and one module (or unit) can also be split into multiple modules (or units).

[0087] Those skilled in the art will appreciate that all or part of the processes in the above-described method embodiments can be implemented by instructing related hardware through a program. The program can be stored in a computer-readable storage medium, and when executed, the program can include the processes in the above-described method embodiments. The storage medium can be a magnetic disk, an optical disk, a read-only memory (ROM), or a random access memory (RAM).

[0088] The above-described embodiments merely represent specific implementations of the present invention. While the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the present invention. It should be noted that a person skilled in the art would be able to make numerous variations and improvements without departing from the spirit of the present invention, and all such variations and improvements fall within the scope of protection of the present invention.

Claims

1. A visual SLAM method based on Manhattan world structure space fusion, characterized by: include: Environmental data is acquired from a visual sensor, and structured line features and spatial cognition information are extracted to obtain vanishing points in three principal directions of the indoor space. The method for extracting the spatial cognition information includes: introducing posture data from an inertial sensor to calculate the vanishing point in the direction of gravity; constructing a unit sphere centered on the camera, sampling on the unit sphere to generate a vanishing point hypothesis in the camera coordinate system, and verifying the vanishing point hypothesis based on the orthogonal relationship of the vanishing points and the line segment response to obtain the optimal vanishing points in the three principal directions; Using spatial cognitive information fusion to correct initial values ​​of triangulation of structured line features; wherein the spatial cognitive information fusion method includes: using vanishing points to classify structured line features detected in the image; if the line is not classified by any vanishing point, converting the Plücker coordinate representation obtained by the current triangulation into an orthogonal representation and using it as the initial value for nonlinear optimization of line parameters; otherwise, correcting the Plücker coordinate representation of the line using the direction of the corresponding vanishing point; Constructing a structured straight line feature error optimization equation, using the corrected straight line coordinates as the initial value of nonlinear optimization, and obtaining SLAM mapping positioning information; wherein the method of constructing the structured straight line feature error optimization equation includes: The Plücker coordinate representation of the current structured feature in the world coordinate system L w =(n w d w ), dw represents the corrected direction vector, n w represents the normal vector of the straight line; and the corresponding vanishing point direction v = (v x v y v z ), construct the residual term r v as follows: The main spatial direction is used to correct the initial value of the structured line feature, and the residual term is constructed to constrain the calculation of SLAM mapping and positioning information.

2. The visual SLAM method based on the fusion of Manhattan world structure space according to claim 1, wherein The extracted structured straight line features at least include straight line features extracted based on the structures of the walls, floors, and ceilings of the indoor space that conform to the Manhattan world structure.

3. The visual SLAM method based on the fusion of Manhattan world structure space according to claim 1, is characterized in that, In the spatial cognition information extraction method, the posture data of the inertial sensor is introduced to calculate the vanishing point in the gravity direction to obtain the optimal vanishing points in the three main directions in the camera coordinate system. The specific method includes: Define the vanishing point in the camera coordinate system as The rotation matrix of the IMU coordinate system at the current moment relative to the first frame is Rs; The rotation matrix from the IMU coordinate system to the camera is R cb ; According to the transformation relationship between coordinate systems, the vanishing point in the world coordinate system is In the camera coordinate system it is expressed as:

4. The visual SLAM method based on Manhattan world structure space fusion according to claim 3, is characterized in that, In the spatial cognitive information extraction method, a unit sphere is constructed with the camera as the center and sampling is performed on the unit sphere to generate a vanishing point hypothesis. The specific method includes: constructing a unit sphere with its X-axis and Y-axis parallel to the X-axis and Y-axis of the image plane, respectively, and the Z-axis pointing to the optical center of the camera. Given the optical center (x0, y0) and focal length f of the camera, the vanishing point pixel coordinates are converted to coordinates (X, Y, Z) on the unit sphere, and polar coordinates are established on the unit sphere to obtain the polar coordinate system angles φ and λ: in, In with The vanishing point hypothesis is generated by uniform sampling on the orthogonal unit sphere plane, with a sampling interval of 1°, that is, a total of 360 vanishing point hypotheses are generated; according to the orthogonal property, the coordinates of the i-th vanishing point hypothesis on the unit sphere are Calculation is done using the following equation: The third vanishing point hypothesis and and They all satisfy the orthogonal relationship and can be directly calculated based on the vector cross product:

5. The visual SLAM method based on Manhattan world structure space fusion according to claim 4, is characterized in that, In the spatial cognitive information extraction method, the method for verifying the vanishing point hypotheses based on the orthogonal relationship of the vanishing points and the line segment response to obtain the optimal vanishing points in three main directions includes: finding a hypothesis with the largest line segment response among 360 generated vanishing point hypotheses, quickly verifying the hypothesis using a polar coordinate grid, and selecting the hypothesis with the largest response value as the optimal vanishing point estimate; and calculating and comparing the Euclidean distance d between the vanishing point coordinates in the current frame and the previous frame, using the vanishing point sorting in the first frame as a reference, thereby classifying and sorting v2 and v3.

6. The visual SLAM method based on Manhattan world structure space fusion according to claim 1, characterized in that, The method for correcting the initial value of the triangulation of the structured straight line feature is as follows: a line is drawn between the midpoint of a pixel line segment of the detected structured straight line feature and the three vanishing points, and the angle δ between the line and the line segment is calculated; if δ is less than a set threshold, the pixel line segment is considered to be consistent with the corresponding vanishing point direction and category; and if the angle between at least two line segments is less than the set threshold, the pixel line segment is considered not to belong to any vanishing point direction; Assume that the classification results of the straight line and the vanishing point If consistent, As the correction value of the direction vector of the straight line in the world coordinate system; After transforming M from the camera coordinate system to the world coordinate system, it is combined with the corrected direction vector d w Perform cross multiplication to update the normal vector n of the line w .

7. A visual SLAM device based on Manhattan world structure space fusion, characterized in that: A method for implementing a visual SLAM method based on Manhattan world structure space fusion as claimed in any one of claims 1 to 6, comprising: The spatial information extraction module is used to obtain environmental data from the visual sensor, perform structured line feature extraction and spatial cognitive information extraction, and obtain the three main direction vanishing points of the indoor space; The spatial information fusion module is used to modify the initial value of the triangulation of structured line features using spatial cognitive information fusion; The error optimization module constructs the structured straight line feature error optimization equation, uses the corrected straight line coordinates as the initial value of nonlinear optimization, and obtains SLAM mapping and positioning information.

Images