Monocular vision method and processing device for a pipeline scene

Abstract

The application discloses a monocular vision method and processing device for a pipeline scene, comprising the following steps: acquiring a plurality of image frames, generating a plurality of first map points according to the plurality of image frames; determining a plurality of key frames in the image frames, obtaining a plurality of second map points by performing feature matching on the plurality of key frames; extracting initial cylindrical features of a first key frame, and calculating cylindrical intersection points of the plurality of key frames according to the initial cylindrical features to obtain a plurality of third map points; optimizing pose information of the plurality of key frames and the plurality of map points, wherein the optimization comprises optimization of a target function combined with point feature constraints and cylindrical feature constraints, the cylindrical feature constraints are obtained based on the initial cylindrical features, and the map points comprise the first map points, the second map points and the third map points; and outputting final pose information and a final point cloud map. The application can improve positioning and mapping accuracy in a pipeline scene.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of monocular vision Simultaneous Localization And Mapping (SLAM), and in particular to a monocular vision method for a pipeline scene and a processing device. BACKGROUND

[0002] Underground water pipelines are one of the important infrastructures for city operation and resident life, and bear the functions of flood discharge, water supply, sewage discharge, etc. According to the statistical data released by the Ministry of Housing and Urban-Rural Development, the length of urban pipelines in China in 2022 is about 3.49 million kilometers, of which the total length of underground water pipelines is about 1.98 million kilometers. Therefore, underground water pipelines are as important to our life as arteries to the human body. However, various and numerous pipeline problems will continue to occur as the use time of the pipelines increases, and if these problems cannot be found and solved, hidden dangers will be caused, leading to economic losses, environmental pollution or health problems. In order to improve the service life of the pipelines and ensure safety, timely detection of the operation state of the pipelines becomes the key to the normal use of the pipelines. In the process of intelligent pipeline inspection by a pipeline unmanned vehicle, the unmanned vehicle needs to autonomously determine the next operation according to the real-time information of the current environment. Therefore, real-time positioning and working environment mapping of the pipeline unmanned vehicle are important links in the intelligent pipeline inspection process.

[0003] Simultaneous Localization And Mapping (SLAM) by monocular vision refers to the technology that a robot simultaneously realizes self-positioning and environment mapping according to the data provided by a monocular camera sensor carried by the robot when moving in an unknown environment without prior information, and is also a basic problem and research hotspot in the field of autonomous positioning and navigation. The monocular vision sensor has the advantages of lightweight, easy installation, low price and rich visual information, and is widely carried on small and low-power pipeline robot platforms.

[0004] Traditional monocular vision SLAM methods mainly rely on feature points in the environment and perform positioning and mapping according to the tracking of the feature points. However, in a weak-texture pipeline scene, image feature points are sparse, and strong light changes or severe motion easily affect the accuracy of the algorithm, and even cause the algorithm to fail. In contrast, there are obvious cylindrical structural features in the pipeline environment, and the visual cylindrical features have higher stability under changes in light and viewing angle, and can provide more rich environmental geometric structure information than visual point features. The SLAM method combining monocular visual point features and cylindrical features can improve the robustness and autonomous positioning accuracy of the entire SLAM method through stable and accurate cylindrical structures. SUMMARY

[0005] The purpose of the present application is to provide a pipeline scene-oriented monocular vision method and processing device to solve the technical problem of low detection accuracy of the traditional monocular vision simultaneous localization and mapping method in weak texture pipeline scenes in the prior art. The technical effects produced by the preferred technical solutions in the various technical solutions provided by the present application are described in detail below.

[0006] To achieve the above-mentioned purpose, the present application provides the following technical solutions:

[0007] The first aspect of the present application provides a pipeline scene-oriented monocular vision method, which is used for simultaneous localization and mapping of the pipeline scene, comprising the following steps: acquiring a plurality of image frames, generating a plurality of first map points according to the plurality of image frames; determining a plurality of key frames in the image frames, obtaining a plurality of second map points by feature matching on a plurality of the key frames; extracting initial cylindrical features of the first key frame, and calculating cylindrical intersection points of a plurality of key frames according to the initial cylindrical features to obtain a plurality of third map points; optimizing the pose information of the plurality of key frames and a plurality of map points, wherein the optimization includes optimization of a target function combined with point feature constraints and cylindrical feature constraints, the point feature constraint is a reprojection error constraint of feature points corresponding to the plurality of map points in the key frame, the cylindrical feature constraint is a constraint obtained based on the point-cylinder distance error of the initial cylindrical features, and the map points include the first map points, the second map points and the third map points; outputting the final pose information of the optimized key frames and the final point cloud map composed of the optimized map points.

[0008] In some embodiments, after the cylindrical intersection points of a plurality of key frames are calculated according to the initial cylindrical features to obtain a plurality of third map points, the method further comprises: verifying the cylindrical features of the current key frame; if the verification is successful, the initial cylindrical features of the first key frame are used as the initial cylindrical features of the next key frame; if the verification fails, the cylindrical features of the current key frame are extracted and used as the initial cylindrical features of the next key frame.

[0009] In some embodiments, the verification of the cylindrical features of the current key frame comprises: calculating the proportion of the cylindrical intersection points in the map points; if the proportion is less than a first preset threshold, it is defined as verification failure.

[0010] In some embodiments, after the pose information of the plurality of key frames and the plurality of map points are optimized, it further comprises: extracting the cylindrical features of the current key frame and using them as the initial cylindrical features of the next key frame.

[0011] In some embodiments, the extracting the cylindrical feature of the current key frame comprises the following steps: obtaining initial values of cylindrical parameters of a candidate cylinder; constructing a cylindrical parameter model if the number of map points used for cylindrical fitting is greater than a second preset threshold; and outputting an estimated result of the cylindrical parameters as the cylindrical feature of the current key frame if the cylindrical fitting is successful.

[0012] In some embodiments, the obtaining the initial values of the cylindrical parameters of the candidate cylinder comprises the following steps: judging whether the current map has a candidate cylinder, the current map comprising a plurality of map points, the candidate cylinder being assigned with the initial values of the cylindrical parameters; creating the candidate cylinder if the current map does not have the candidate cylinder; and taking the cylindrical parameters of the candidate cylinder as the initial values of the cylindrical parameters if the current map has the candidate cylinder.

[0013] In some embodiments, the taking the cylindrical parameters of the candidate cylinder as the initial values of the cylindrical parameters comprises the following steps: obtaining a transformation relationship between a world coordinate system and a cylindrical coordinate system, the transformation relationship comprising a rotation matrix and a translation vector; determining the cylindrical coordinate system and obtaining the initial values of the cylindrical parameters according to the transformation relationship.

[0014] In some embodiments, the constructing the cylindrical parameter model comprises the following steps: transforming the map points in the world coordinate system to the cylindrical coordinate system, obtaining a point-to-cylinder distance error function based on the rotation matrix and the translation vector; performing Jacobian derivation on the point-to-cylinder distance error function to obtain a Jacobian matrix; constructing a graph optimization model according to the Jacobian matrix, solving the graph optimization model, and obtaining the cylindrical parameter model.

[0015] In some embodiments, the objective function satisfies the following formula:

[0016]

[0017] wherein e ij ∈R 2 represents a re-projection error of a feature point j corresponding to the map point in a key frame i, e mn ∈R represents a point-to-cylinder distance error of a cylinder intersection n in a cylinder m, L k is a key frame set composed of the kth key frame and the co-visible key frames of the kth key frame, Q m is a set of cylinder intersections corresponding to the cylinder m, is an inverse of a noise covariance matrix of visual feature observation, is an inverse of a noise covariance matrix of point-to-cylinder distance, and ρ(·) is a Cauchy robust kernel function, and argmin represents a value of a variable x when the objective function reaches a minimum value.

[0018] A second aspect of the present application provides a processing device, comprising: one or more processors; a memory for storing one or more computer programs, and one or more processors are configured to execute one or more computer programs stored in the memory, so that one or more processors execute the monocular vision method for the pipeline scene as described above.

[0019] Implementing one of the technical solutions of the above-mentioned technical solutions of the present application has the following advantages or beneficial effects: in the present application, since in the pipeline scene, the pipeline can be approximately regarded as a cylinder, by extracting the initial cylinder feature of the first key frame and calculating the intersection points of the cylinder of multiple key frames to obtain the map point, the map point can be more fitted to the application scene; at the same time, when optimizing, the point feature constraint and the cylinder feature constraint are combined to optimize the position information of the map point and the pose information, so as to improve the positioning and mapping accuracy in the pipeline scene. BRIEF DESCRIPTION OF DRAWINGS

[0020] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings needed in the embodiment description will be briefly introduced as follows. Obviously, the drawings in the following description are only some embodiments of the present application, and other drawings can also be obtained according to these drawings without creative labor, and the drawings are as follows:

[0021] Figure 1 is a flowchart of the embodiment of the present application;

[0022] Figure 2 is a schematic diagram of the relationship between the world coordinate system and the cylindrical coordinate system of the embodiment of the present application;

[0023] Figure 3 is a structural block diagram of the processing device of the embodiment of the present application;

[0024] Figure 4A is a comparative schematic diagram of the positioning result trajectory error of the embodiment of the present application; Figure 4B is a comparative schematic diagram of the error on the xyz three-axis component of the embodiment of the present application; Figure 4C is a comparative schematic diagram of the error on the rpy rotation component of the embodiment of the present application.

[0025] In the figure: 1, processing device; 10, memory; 11, processor. DETAILED DESCRIPTION

[0026] In order to make the purposes, technical solutions and advantages of the present application clearer, the various exemplary embodiments to be described below will be described with reference to the corresponding drawings, which form part of the exemplary embodiments, and in which various exemplary embodiments that can be employed to implement the present application are described. Unless otherwise indicated, the same numbers in different drawings indicate the same or similar elements. The implementations described in the following exemplary embodiments do not represent all the implementations consistent with the present disclosure. It should be understood that they are only examples of processes, methods and apparatuses, etc. consistent with some aspects of the present disclosure as detailed in the appended claims, and other implementations can be used or structural and functional modifications can be made to the implementations listed herein without departing from the scope and spirit of the present application.

[0027] In the description of the present application, it should be understood that the terms "center", "longitudinal", "transverse" and the like indicate the orientation or positional relationship shown in the drawings, which is only for the convenience of describing the present application and simplifying the description, and does not indicate or imply that the elements referred to must have a specific orientation, be constructed and operated in a specific orientation. The terms "first", "second" and the like are only for descriptive purposes and cannot be understood as indicating or implying relative importance or implicitly indicating the number of technical features indicated. The term "a plurality of" means two or more. The terms "connected", "connected" should be interpreted broadly, for example, it can be fixed connection, detachable connection, integral connection, mechanical connection, electrical connection, communication connection, direct connection, indirect connection through intermediate medium, internal communication of two elements or interaction relationship between two elements. The term "and / or" includes any and all combinations of one or more related listed items. For those skilled in the art, the specific meaning of the above terms in the present application can be understood according to the specific circumstances.

[0028] In order to illustrate the technical solutions described in the present application, the following will be described by specific embodiments, only showing parts related to the embodiments of the present application.

[0029] As shown in Figure 1 The first aspect of the present application provides a monocular vision method for a pipeline scene, comprising the following steps:

[0030] S1, acquiring a plurality of image frames, and generating a plurality of first map points according to the plurality of image frames.

[0031] In some embodiments, a plurality of image frames arranged in time sequence can be acquired. Specifically, a plurality of image frames taken in the pipeline scene can be acquired by a monocular camera, and the plurality of image frames can be arranged into time sequence data in time sequence. Further, the monocular camera can be installed on a pipeline robot, and the plurality of image frames can be acquired by the pipeline robot in the pipeline scene.

[0032] After obtaining the plurality of image frames, the method further includes: pre-processing the plurality of image frames. In some embodiments, the pre-processing can include at least one of de-distortion processing, image pyramid, gray centroid method, and uniformization strategy. Specifically, the feature points can be processed in sequence through the image pyramid, the gray centroid method, and the uniformization strategy. The image pyramid is a multi-scale expression of the image frames, i.e., a structure of the image frames is explained in multiple resolutions, thereby ensuring scale invariance of the feature points; the gray centroid method can obtain a centroid coordinate based on a gray distribution of the image frames, thereby ensuring rotation invariance of the feature points; and the uniformization strategy can make the feature points more uniformly distributed in the entire image frame, i.e., each position region in the image frame can have at least one feature point, thereby avoiding excessive concentration of the feature points.

[0033] In some embodiments, the plurality of image frames can be de-distortion processed according to an intrinsic matrix and distortion coefficients of the monocular camera pre-calibrated. The intrinsic matrix of the monocular camera can be defined by the following formula:

[0034]

[0035] It should be noted that the above formula is obtained from a pinhole camera model, i.e., the imaging process of the pinhole camera can be expressed. Specifically, the above formula can describe the process of projecting a three-dimensional point in a world coordinate system to an image plane, and then obtaining a two-dimensional point coordinate in a pixel coordinate system. Wherein, [u, v] T represents the coordinates of the feature points in the pixel coordinate system, [X c ,Y c ,Z c ] T represents the coordinates of the three-dimensional point in the camera coordinate system, [X w ,Y w ,Z w ] T represents the coordinates of the three-dimensional point in the world coordinate system; f x = αf, f y = βf, α, β are scaling factors, f is the focal length of the camera, c x , c y represent the translation of the origins of the two coordinate systems in the x and y directions, respectively. The matrix formed by the four is the intrinsic matrix; I is a 3x3 identity matrix, which functions to convert the homogeneous coordinates into non-homogeneous coordinates; R is a rotation matrix, and t is a translation vector. The matrix formed by the rotation matrix and the translation vector can also be referred to as an extrinsic matrix, which can transform the three-dimensional point from the world coordinate system to the camera coordinate system.

[0036] In some embodiments, the distortion coefficients can include a radial distortion coefficient and a tangential distortion coefficient. A distortion model can be established based on the intrinsic matrix and the distortion coefficients, and the distortion model can be used for distortion rectification of the image frames.

[0037] In some embodiments, feature points of the plurality of image frames can be extracted and descriptors can be calculated, where the descriptors can be used to quantify information of the feature points.

[0038] In some embodiments, the feature points can be ORB feature points; the feature points can refer to representative regions in the image frames, where the representative regions in the image frames can refer to at least one of a corner point, an edge, and a block in the image frames.

[0039] In some embodiments, feature matching can be performed on the feature points of each two of the image frames to obtain pose information of the current image frame, and a plurality of first map points can be generated based on the pose information to obtain an initial map. The first map points can be three-dimensional map points, and the initial map can include a plurality of three-dimensional map points, such as the first map points.

[0040] In some embodiments, feature matching can be performed on the feature points of each two of the image frames to obtain matching point pairs. Specifically, a plurality of feature points in the current image frame and the adjacent frame can be denoted as Ki and Qj, respectively, where i, j = 1, 2, 3…; then, feature matching is performed on the nearest feature points in K1 and Qj, feature matching is performed on the nearest feature points in K2 and Qj, and so on, until feature matching of the plurality of feature points is completed to obtain the matching point pairs.

[0041] In some embodiments, the matching point pairs between the first two image frames can be obtained, one of a fundamental matrix or a homography matrix can be calculated based on the epipolar geometry principle (binocular imaging principle), and then the fundamental matrix or the homography matrix can be decomposed to obtain pose information of the current image frame, three-dimensional map points can be generated based on the pose information to obtain an initial map. The initial map can be composed of discrete three-dimensional map points, such as the first map points, and can be a point cloud map.

[0042] In some embodiments, the matching point pairs can be triangulated based on the matching point pairs and the pose information to generate the first map points. Since in monocular visual SLAM, the depth information of the map points cannot be obtained from a single image, the depth information of the map points can be obtained by triangulation to obtain the first map points.

[0043] The above triangulation satisfies the following formula:

[0044] s2x2=s1Rx1+t,

[0045] wherein x1, x2 are two matched feature points, i.e. normalized coordinates of a matched point pair; and R, t are pose information of the current image frame. Thus, depth information of the two feature points x1, x2 can be solved, and thus the first map point can be obtained.

[0046] In some embodiments, the pose information of each image frame and the corresponding first map point can be obtained, and the initial pose information can be updated.

[0047] In some embodiments, the initial pose information can refer to the pose information of the first image frame or the first key frame. The initial pose information can be saved as the first key frame.

[0048] In some embodiments, after the plurality of first map points are generated according to the plurality of image frames, the method further comprises: obtaining the pose information of each image frame, and updating the initial pose information. The updating manner can comprise at least one of key frame tracking, constant speed model tracking, repositioning tracking, and local map tracking. In some embodiments, the key frame tracking can refer to predicting the position of the current image frame by using the image frame whose position has been determined, and thus updating the initial pose information, which can be used for the first few frames of the plurality of image frames, such as the first image frame. The constant speed model tracking can refer to updating the initial pose information by integrating the motion between the continuous plurality of image frames, assuming constant speed. The repositioning tracking can refer to repositioning by searching the previous image frame. The local map tracking can refer to projecting the three-dimensional map points in the local map observed by the local key frame of the current image frame into the current image frame, and optimizing the initial pose information by more matched feature points.

[0049] S2, determining a plurality of key frames in the image frames, and obtaining a plurality of second map points by performing feature matching on the plurality of key frames.

[0050] In some embodiments, the key frame can refer to a frame most representative in the plurality of continuous image frames, having higher stability, richer information amount, and greater difference from adjacent frames. The key frame can be selected periodically from the plurality of continuous image frames. In some embodiments, a key frame can be selected from the continuous image frames at a preset time interval, and the preset time interval can be 0.5 seconds, 0.75 seconds, or 1 second, which is not limited here and can be set according to actual needs. In some embodiments, two adjacent key frames can be apart by a preset number of frames, and the preset number of frames can be greater than 5. Thus, the similarity of the images seen by two key frames close to each other can be prevented from being high, thereby affecting the mapping precision.

[0051] In some embodiments, after determining multiple keyframes in an image frame, the method further includes: filtering the first map point; if the reprojection error of the first map point in the current keyframe is greater than a one-pixel deviation, then deleting the first map point.

[0052] In some embodiments, newly generated first map points from the previous image frame can be deleted through filtering. In other embodiments, the first map point is deleted if it is detected that it is not preceding the current keyframe. In this case, since newly added first map points in monocular mode require a relatively strict filtering process to remain, the accuracy of mapping and localization can be guaranteed.

[0053] In some embodiments, feature matching can be re-performed on feature points of multiple keyframes, and triangulation can be performed on the re-matched feature points to obtain multiple second map points. Feature matching can be re-performed on feature points among shared keyframes to generate second map points. The second map points can be 3D map points.

[0054] In some embodiments, a shared-view keyframe refers to a keyframe that can observe the same 3D map points as the current keyframe. That is, shared-view keyframes can be used to describe whether there is a shared-view relationship between various keyframes. Connecting multiple keyframes with shared-view relationships can form a nature graph. The vertices of the nature graph represent the pose information of the keyframes, and the edges represent the connections between two keyframes.

[0055] S3. Extract the initial cylindrical features of the first keyframe, and calculate the cylindrical intersections of multiple keyframes based on the initial cylindrical features to obtain multiple third map points.

[0056] In some embodiments, the intersection points of cylinders in multiple keyframes can be calculated to obtain a third map point. The third map point can be a cylinder map point. The cylinder involved in this application can be considered as the entire cylinder corresponding to the internal structure of a pipe in a pipe scene. In some embodiments, the map point includes a first map point, a second map point, and a third map point.

[0057] In some embodiments, the cylindrical intersection point can be calculated by obtaining the intersection point of the back-projected ray of the feature point in the keyframe with the cylindrical surface. The parametric equation of the back-projected ray satisfies:

[0058]

[0059] Where, [x,y,z] T The coordinates represent the intersection points of the cylinders on the straight line, [x1, y1, z1]. T The coordinates represent the coordinates of the optical center O1 of the first keyframe in the figure in the cylindrical coordinate system, [v x ,v y ,v z] T It is the direction vector of the straight line formed by the normalized coordinates of the optical center and the feature point m1.

[0060] The parametric equations of points on a cylindrical surface satisfy:

[0061] x 2 +y 2 =r 2 ,

[0062] Where, [x,y] T This represents the coordinates of a point on the cylindrical surface in the XOY plane of the cylindrical coordinate system, where r is the radius of the cylinder.

[0063] Combining the parametric equations of the back-projected ray and the parametric equations of the points on the cylindrical surface, we obtain the following formula:

[0064]

[0065] Where, [x,y,z] T The coordinates represent the intersection points of the cylinders on the straight line, [x1, y1, z1]. T The coordinates represent the coordinates of the optical center O1 of the first keyframe in the figure in the cylindrical coordinate system, [v x ,v y ,v z ] T Let [x, y] be the direction vector of the line formed by the optical center and the normalized coordinates of feature point m1. T This represents the coordinates of a point on the cylindrical surface in the XOY plane of the cylindrical coordinate system, where r is the radius of the cylinder. The coordinates of the cylindrical intersection point in the cylindrical coordinate system can be obtained using the above formula. Since the cylindrical map point lies both on the straight line and on the cylindrical surface, this cylindrical intersection point is the cylindrical map point, which is also the third map point.

[0066] In some embodiments, after calculating the cylindrical intersections of multiple keyframes based on the initial cylindrical features to obtain multiple third map points, the method further includes: verifying the initial cylindrical features of the current keyframe; if the verification is successful, then using the initial cylindrical features of the first keyframe as the initial cylindrical features of the next keyframe; if the verification fails, then extracting the cylindrical features of the current keyframe and using them as the initial cylindrical features of the next keyframe.

[0067] In some embodiments, verifying the cylindrical features of the current keyframe includes: calculating the proportion of cylindrical intersections to map points; if the proportion is less than a first preset threshold, it is defined as verification failure. The proportion of the total number of cylindrical intersections to the total number of map points can be calculated. Thus, the accuracy of the current cylindrical structure can be verified based on the proportion. Correspondingly, if the proportion is greater than the first preset threshold, it is defined as verification success, and the initial cylindrical features of the first keyframe can be applied to subsequent keyframes.

[0068] S4, optimizing the pose information of the plurality of key frames and the plurality of new map points, wherein the optimization comprises optimization of an objective function combining point feature constraints and cylindrical feature constraints, the point feature constraints being reprojection error constraints of feature points corresponding to the plurality of map points in the key frames, and the cylindrical constraints being constraints obtained based on point-cylinder distance errors of the initial cylindrical features.

[0069] In some embodiments, the optimization can include at least one of a local bundle adjustment optimization, a gradient descent method, a stochastic gradient descent method, a quasi-Newton method, a Levenberg-Marquardt algorithm, and an iterative closest point algorithm.

[0070] In some embodiments, the pose information of the plurality of key frames and the map points can be subjected to local BA (Bundle Adjustment) optimization. The local BA optimization can combine the point feature constraints and the cylindrical feature constraints. The point features can be used to establish a connection between different key frames to obtain the pose information.

[0071] In some embodiments, the state to be optimized composed of the point features and the cylindrical features is as follows:

[0072]

[0073] wherein V k represents the co-viewed key frames of the kth key frame, P k represents the map points observed by the kth key frame, I k represents the cylinders related to the kth key frame, T iw ∈ SE(3) represents the pose parameters of the key frames in the world coordinate system, p j represents the three-dimensional coordinate parameters of the map points, Y m represents the cylindrical parameters, T iw wherein i ∈ V k represents the camera coordinate system of a frame in the co-viewed key frames of the key frame, and w represents the world coordinate system.

[0074] In some embodiments, the point feature constraints can refer to the reprojection error constraints of the feature points, and the cylindrical features can refer to the distance error constraints of the points to the cylindrical surface; the two error constraints are combined together to form the following objective function:

[0075]

[0076] wherein e ij ∈ R 2 represents the reprojection error of the feature point j corresponding to the map point in the key frame i, e mn ∈ R represents the point-cylinder surface distance error of the intersection point n in the cylindrical m, and Lk is a keyframe set composed of the k-th keyframe and its co-visible keyframes, Q m is a set of corresponding cylindrical points on the cylinder m. is the inverse of the noise covariance matrix of the visual feature observation, is the inverse of the noise covariance matrix of the point-to-cylinder distance, and ρ(·) is the Cauchy robust kernel function, and argmin represents the value of the variable x when the objective function reaches the minimum value. By minimizing the objective function, the optimal estimate of the state X is obtained.

[0077] In some embodiments, the reprojection error can satisfy the following formula:

[0078]

[0079] where u ij is the observation value of the feature point j on the keyframe i, K is the camera intrinsic parameter, T is the camera extrinsic parameter, i.e., the pose information, and p j is the corresponding map point coordinate of the feature point j.

[0080] In some embodiments, if the reprojection error is within a pixel error range, the cylindrical map point can be considered to be qualified; if the reprojection error is greater than a pixel error, a second map point is generated by triangulation. That is, if the generated cylindrical map point satisfies the reprojection error, the second map point will not be generated by triangulation; correspondingly, if the generated cylindrical map point does not satisfy the reprojection error, the second map point is generated by triangulation.

[0081] In some embodiments, after optimizing the pose information of the plurality of keyframes and the plurality of map points, the cylindrical features of the current keyframe are extracted and used as the initial cylindrical features of the next keyframe. Further, if the verification of the initial cylindrical features of the current keyframe fails, the cylindrical features of the current keyframe can be extracted after the above optimization. Thus, the extracted cylindrical features can be more accurate.

[0082] In some embodiments, after optimizing the pose information of the plurality of keyframes and the plurality of map points, the plurality of keyframes are screened and the plurality of redundant keyframes are deleted.

[0083] Generally, the SLAM system only needs to output one accurate camera trajectory, so it is necessary to delete the redundant camera trajectories. During the process of obtaining image frames, the monocular camera may be blocked, shaken and the like, so that the tracking is frequently lost, and then the SLAM system generates multiple trajectories. The redundant trajectory can refer to a trajectory that fails to form a loop with the initial trajectory, and the key frame on the redundant trajectory can be a redundant key frame. For example, if the trajectory label before the first tracking loss is 0, the label of the trajectory generated by the local initialization algorithm is gradually incremented, and at this time, the trajectory with a label other than 0 can be deleted.

[0084] In some embodiments, all the redundant key frames on the redundant trajectory can be deleted on a key frame database, where the key frame database can be a database system for storing and managing key frame information, and then the point cloud map corresponding to the redundant key frames can be deleted. In this case, since too many key frames will cause the local BA optimization to be very slow, by deleting the key frames with a higher degree of redundancy, the size of the local BA optimization can be reduced.

[0085] Extracting the cylindrical feature of the current key frame can include the following steps: obtaining the initial value of the cylindrical parameter; if the number of three-dimensional map points used for cylindrical fitting is greater than a second preset threshold, constructing a cylindrical parameter model; and if the cylindrical fitting is successful, outputting the estimated result of the cylindrical parameter as the cylindrical feature of the current key frame.

[0086] In some embodiments, obtaining the initial value of the cylindrical parameter of the candidate cylinder can include the following steps: determining whether the current map has a candidate cylinder, the current map including a plurality of map points, and the candidate cylinder being assigned with the initial value of the cylindrical parameter; if the current map does not have a candidate cylinder, creating a candidate cylinder; and if the current map has a candidate cylinder, taking the cylindrical parameter of the candidate cylinder as the initial value of the cylindrical parameter.

[0087] In some embodiments, before obtaining the initial value of the cylindrical parameter, it can be determined whether the current map has a candidate cylinder. In some embodiments, the cylinder can include a candidate cylinder with a candidate state and an active cylinder with an active state. The candidate state of the cylinder means that the current cylinder has been created, but a reasonable cylinder has not been estimated. When a reasonable cylinder is estimated, the state of the cylinder can be set to the active state for subsequent use.

[0088] In some embodiments, taking the cylindrical parameter of the candidate cylinder as the initial value of the cylindrical parameter can include the following steps: obtaining the transformation relationship between the world coordinate system and the cylindrical coordinate system, the transformation relationship can include a rotation matrix and a translation vector; and determining the cylindrical coordinate system and obtaining the initial value of the cylindrical parameter according to the transformation relationship.

[0089] In some embodiments, obtaining initial values ​​for cylinder parameters may include: adaptively assigning initial values ​​to the given cylinder parameters; and collecting cylinder fitting input points. This application obtains the rotational relationship between the Z-axis of the two coordinate systems through Principal Component Analysis (PCA), and then uses the distance from the midpoint to the line in three-dimensional space to obtain the position of the spatial cylinder and the rotational relationship between the X-axis of the two coordinate systems. This orientation and position information is used as the initial values ​​for the cylinder parameters. Simultaneously, three-dimensional map points optimized through multiple local BA iterations are collected as cylinder fitting input points. These cylinder fitting input points can be used for cylinder fitting.

[0090] In some embodiments, a transformation relationship between the world coordinate system and the cylindrical coordinate system can be established. The transformation relationship between the orientation of the cylindrical coordinate system and the orientation of the world coordinate system can be described as follows:

[0091]

[0092] R cy2cy1 Represents the cylindrical coordinate system X c The rotational relationship between the axis and the X-axis of the world coordinate system, R cy1w Represents the cylindrical coordinate system Z c The rotational relationship between the axis and the Z-axis of the world coordinate system. The two coordinate systems pass through R... cyw After rotation, only the orientation remains the same; then, they are translated by a distance t. x Distance is used to obtain a cylindrical coordinate system.

[0093] Specifically, such as Figure 2 As shown, the cylindrical coordinate system designed in this application establishes a cylindrical coordinate system CX within the world coordinate system O-XYZ. c Y c Z c CZ in cylindrical coordinate system c The axis is the direction of the fitted spatial cylinder axis. The origin C of the cylindrical coordinate system is the perpendicular point from the origin O of the world coordinate system to the cylinder axis. The CX axis of the cylindrical coordinate system... c The axis passes through the origin of the world coordinate system, CX c The positive direction of the axis is the direction of vector CO. When the CZ axis of the cylindrical coordinate system... c With CX c When determined, the cylindrical coordinate system can be uniquely determined.

[0094] like Figure 2 As shown, in the relationship between the cylindrical coordinate system and the world coordinate system constructed in this application, it can be known that the translation distance t x This is the distance between the origins of the two coordinate systems. In some embodiments, the initial value can be a preset value, such as a rotation variable. Translation distance t x The initial values ​​can all be 0.

[0095] In some embodiments, it can be determined whether the number of three-dimensional map points for the cylinder fitting is greater than a second preset threshold. If the number of three-dimensional map points for the cylinder fitting is greater than the second preset threshold, in the next key frame, the three-dimensional map points that have undergone multiple local BA optimizations are continuously collected as cylinder fitting input points.

[0096] In some embodiments, constructing the cylinder parameter model can include the following steps: transforming the map points in the world coordinate system to the cylindrical coordinate system, obtaining a point-cylinder surface distance error function based on the rotation matrix and the translation vector; performing Jacobian derivation on the point-cylinder surface distance error function to obtain a Jacobian matrix; constructing a graph optimization model according to the Jacobian matrix, solving the graph optimization model, and obtaining the cylinder parameter model.

[0097] In some embodiments, the cylinder parameter model can be used to obtain a cylinder parameter result, and the cylinder parameter result is the cylinder feature.

[0098] In some embodiments, if the number of three-dimensional map points for the cylinder fitting is greater than the second preset threshold, a cylinder parameter model is constructed. Specifically, a graph optimization model of the cylinder fitting algorithm can be constructed according to the point-cylinder surface distance error function and the Jacobian matrix proposed in the present application, the error function is optimized and solved using the g2o library, and the cylinder parameter model is obtained. g2o (General Graphic Optimization) is a general library based on graph optimization, and can solve a least square problem represented as a graph model through g2o.

[0099] In some embodiments, after obtaining the rotation matrix and the translation vector between the cylindrical coordinate system and the world coordinate system, the three-dimensional points in the world coordinate system can be transformed to the cylindrical coordinate system to obtain a point-cylinder surface distance error function:

[0100]

[0101] where ||·||2 represents the 2-norm of a vector, p i is the three-dimensional point coordinate in the world coordinate system, the coordinate in the cylindrical coordinate system is obtained after rotation and translation, the first two dimensions of the coordinate are taken out, the distance to the Z-axis of the cylindrical coordinate system is calculated, and the distance of the three-dimensional point to the cylindrical surface is obtained by subtracting the cylindrical radius.

[0102] In some embodiments, the objective function of the fitted cylinder is:

[0103]

[0104] represents the cylinder parameters, and the least square problem is solved to estimate the cylinder parameters.

[0105] It should be noted that there are two ways to express the rotation relationship between the cylindrical coordinate system and the world coordinate system, one is R cyw , which is to express the rotation in the form of a rotation matrix; at the same time, the rotation can also be expressed in the form of Lie algebra, that is The expanded form is , that is, only three quantities are composed. In general, R cyw is usually used to express the rotation in the error function expression, and is usually used to express the rotation when expressing the cylindrical parameters and the Jacobian matrix. It can be understood that here only the rotation can have different forms of expression, and no matter what parameters or forms are used to express the rotation, it does not constitute a limitation on the present application.

[0106] When solving the nonlinear least squares estimation problem, it is necessary to linearly expand the error function near the current estimated value, that is, to solve the Jacobian matrix J(w) of the error function e(w) with respect to the variable w. When deriving the Jacobian, the point cylindrical distance error function is arranged as follows:

[0107]

[0108] Let y = (R cyw p) T A(R cyw p)+(R cyw p) T t cyw +t cyw T (R cyw p)+t x 2 , wherein the matrix A is used to take out the first two coordinates of the three-dimensional point in the cylindrical coordinate system, R cyw is a rotation matrix, p = [x w , y w , z w ] T is the coordinate of the three-dimensional point in the world coordinate system in space, t cyw is a translation vector, and the translation vector only has a value in the X-axis direction, so t cyw = [t x , 0, 0] T , and r is the radius of the cylindrical structure.

[0109] In some embodiments, the matrix A can be expressed as:

[0110]

[0111] The derivative of the error function e with respect to the rotation Lie algebra is a 1x3 matrix, and the derivative of the error function e with respect to the translation variable tx The derivative result is a 1-dimensional number, and the derivative result of the error function e with respect to the radius r is also a 1-dimensional number. Therefore, the Jacobian matrix of the error function e with respect to the cylindrical parameter w can be:

[0112]

[0113] Because the derivative result of the error function e with respect to the Lie algebra of rotation is a 1x3 matrix, so is a 1x3 matrix, and this derivative result contains 3 parameters as a whole; indicates a parameter; -1 indicates a parameter; a total of 5 parameters express the cylindrical characteristics. B = [1, 0, 0] T , the first element of the 1x3 vector is extracted.

[0114] In some embodiments, the Jacobian matrix is a derivative result, and the subsequent graph optimization model can use the derivative result. The process of nonlinear optimization of cylindrical fitting needs the derivative relationship to guide the optimization direction.

[0115] In some embodiments, according to the state returned by the cylindrical fitting, it is determined whether the current cylindrical estimation is successful. If the result is no, the number of times of failure of the current cylindrical estimation is incremented. When the number of estimation failures is too large, the erroneous cylindrical fitting map points can be emptied in time. If the result is yes, the cylindrical parameter estimation result is output. At the same time, the state of the current cylinder can be modified to an active state.

[0116] In some embodiments, the cylindrical feature can be extracted before local BA optimization is performed. Specifically, the cylindrical feature can be extracted in the first key frame. In the second key frame and subsequent key frames, the cylindrical feature is extracted after local BA optimization is performed. In some embodiments, if the cylindrical constraint in the local BA optimization is successful, the cylindrical feature does not need to be continuously extracted; if the cylindrical constraint in the local BA optimization is not successful, the cylindrical feature is continuously extracted in the next key frame.

[0117] In some embodiments, after the pose information of the plurality of key frames and the plurality of map points are optimized, the method can include: performing loop closure detection and loop closure correction on the key frames and the three-dimensional map points.

[0118] In some embodiments, the loop closure detection can include a loop closure candidate key frame set, verifying the loop closure candidate key frame, and calculating a Sim(3) transformation (similarity transformation in three-dimensional space). The purpose of the loop closure detection is to find an accurate candidate key frame and calculate the Sim(3) transformation between the current key frame and the candidate key frame, that is, the total error.

[0119] In some embodiments, the set of loop closure candidate keyframes can be found by querying the bag-of-words database to obtain the candidate keyframes where the loop closure occurs. Specifically, the feature descriptors of the current keyframe and the feature points in the keyframe can be clustered, where the feature descriptors are used to describe the detected feature points, and a node tree is constructed, and then a TF-IDF (term frequency-inverse document frequency) method is used to obtain a bag-of-words (BoW) model of each keyframe. Then, the bag-of-words model is used to calculate the similarity, and the candidate keyframes are determined according to the similarity values, and further, the loop closure relationship is determined according to the matching results of the feature descriptors of the current keyframe and the candidate keyframes.

[0120] Since the monocular camera does not have scale information, the Sim(3) is actually calculated by calculating the rotation, translation and scale factor, so the calculated Sim(3) transformation is the overall error of the pose information; then, the overall error of the pose information is distributed to the remaining keyframes, and the distribution standard is:

[0121] The pose error and scale error in the Sim(3) transformation between the two frames are distributed to the current keyframe and the connecting keyframes, as shown in the following formula:

[0122] g2oCorrentedSiw=g2oSic*mg2oScw,

[0123] Where g2oCorrentedSiw represents the Sim(3) transformation of a connecting keyframe of the current keyframe in the world coordinate system after correction, g2oSic is the Sim(3) transformation between the connecting keyframe and the current keyframe, and mg2oScw is the Sim(3) transformation from the world coordinate system to the current keyframe calculated in the first stage. The connecting keyframe (ConnectingKeyframes) refers to the keyframes that are associated by Sim(3) transformation at different time points.

[0124] In some embodiments, the loop closure correction can include at least one of Sim(3) transformation propagation and correction of the current keyframe, map point coordinate propagation and correction, and essential graph optimization. The loop closure correction is used to distribute the error to all keyframes, and further optimize all keyframes and map points.

[0125] As described above, while obtaining the Sim(3) transformation, the map points of the current key frame are optimized, and it can be considered that the coordinates of the map points of the current key frame are also more accurate, and the coordinate information of the map points connected in the key frame is corrected based on this. Specifically, the coordinate information eigP3Dw of the map points not corrected in the world coordinate system can be obtained, the eigP3Dw is transformed to the camera coordinate system of the key frame through the Sim(3) transformation g2oSiw not subjected to pose propagation, and then the inverse transformation g2oCorrectedSiw of the corrected Sim(3) transformation pose is used to transform the eigP3Dw to the world coordinate system, to obtain the coordinate information of the map points after the Sim(3) pose correction.

[0126] S5, output the final pose information of the optimized key frame and the final point cloud map including the optimized map points. Further, global BA optimization can be performed on all key frames and map points, and the optimized information can be visualized to form a globally consistent map. The globally consistent map can be a point cloud map, which can be a point cloud map constructed after the pipeline robot runs for a period of time. The globally consistent map can be an overall map composed of first map points, such as initial map points, and subsequent continuously generated map points, such as second map points and third map points.

[0127] In the present application, since in the pipeline scene, the pipeline can be approximately regarded as a cylinder, by extracting the initial cylindrical features of the first key frame and calculating the intersection points of the cylinders of multiple key frames to obtain the map points, the map points can be more consistent with the application scene. At the same time, when optimizing, the point feature constraint and the cylindrical feature constraint are combined to optimize the pose information and the position information of the map points, so as to improve the positioning and mapping accuracy in the pipeline scene.

[0128] As shown in Figure 3 The present application also provides a processing device 1, comprising one or more processors 11; a memory 10 for storing one or more computer programs, and the one or more processors 11 are used to execute the one or more computer programs stored in the memory 10, so that the one or more processors 11 execute the monocular vision method for pipeline scene as described in the first aspect of the present application.

[0129] Embodiment:

[0130] The present application uses the MARK-1 of Qibing as a robot chassis for development, carries the RealSense D435i camera sensor component, builds a physical pipeline robot platform based on the Intel x86 processor as the calculation and processing center, and uses the pearl cotton foam board to build a physical experiment scene. The pipeline robot collects monocular image frames as raw data in a long pipeline with turns, and the dataset contains 2127 timestamped RGB image frames.

[0131] In this embodiment, the number of candidate cylinder estimation failures is set to 3 times, the threshold value t of the number of cylinder fitting points is 400, the standard deviation of the distance between the points and the cylinder surface is used to judge whether the current cylinder is estimated successfully, the threshold value of the standard deviation is set to 0.02, and the proportion of the intersection point of the cylinder structure is set to 0.4. In the camera model, the camera intrinsic parameters are involved, and it is considered that the camera intrinsic parameters are fixed after the factory and will not change in the subsequent use process. At the same time, the lens will affect the diffusion of light during imaging, causing distortion phenomenon. The camera intrinsic parameter matrix K and the distortion coefficient need to be obtained through calibration, and the result is:

[0132]

[0133] [k1 k2 p1 p2]=[0.0683-0.21440.0005-0.0067],

[0134] In this embodiment, the maximum error (Max), mean error (Mean), median error (Median), minimum error (Min), standard deviation error (Std) and root mean square error (RMSE) of the APE (Absolute Pose Error) index are used as the standard for measuring the positioning error. The root mean square error calculation method of the APE index is:

[0135]

[0136] wherein, is the absolute error trajectory of the i-th frame, wherein Q1,...,Q n ∈SE(3) is the true value of the trajectory pose of the robot, P1,...,P n ∈SE(3) is the pose estimated by the visual SLAM algorithm, and Δ is the time interval.

[0137] In this embodiment, the error results of the present application and other methods are shown in Table 1 (unit: centimeter).

[0138]

[0139] Table 1

[0140] As shown in Table 1, the monocular vision SLAM method based on point-cylinder combination for pipeline scene in the present application is superior to the mainstream method in various error indicators. Taking RMSE as an example, which can better measure the overall average performance of the model, compared with the ORB-SLAM2 method without adding cylinder features, the positioning accuracy of the method in the embodiment is improved by about 46.19%.

[0141] Figure 4A is a positioning result trajectory error comparison diagram of the embodiment; Figure 4B is an error comparison diagram on the xyz three-axis component; Figure 4C is an error comparison diagram on the rpy rotation component. It should be noted that the embodiment of the present application is PCY-SLAM in Figure 4A . The dotted line represents the trajectory true value, the trajectory obtained by the method of the present application is represented by the red curve, and other colors represent other different methods. As shown in Figure 4A , Figure 4B and Figure 4C , due to the limited experimental site and the short distance of the real pipeline built, only about 10 meters, the positioning errors of all methods are close to the true value, so the trajectory diagrams of the three methods are all coincided with the true value. But in the error comparison diagram on the xyz three-axis component, that is Figure 4B and the error comparison diagram on the rpy rotation component, that is Figure 4C , the results obtained by the present application are closer to the true value, indicating that the autonomous positioning performance of the present application is more accurate.

[0142] Those skilled in the art can understand that all or part of the features / steps of the above-mentioned method embodiments can be realized by a method, a data processing system or a computer program, and these features can be realized in a hardware manner, a software manner or a combination of hardware and software. The foregoing computer program can be stored in one or more computer-readable storage media, and the storage medium stores the computer program, which is executed by (such as a processor) to execute the steps of the monocular vision method for pipeline scene embodiment described above.

[0143] The foregoing storage medium that can store program codes includes: a static hard disk, a solid state disk, a random access memory (SRAM), an electrically erasable programmable read-only memory (EEPROM), an erasable programmable read-only memory (EPROM), a programmable read-only memory (PROM), a read-only memory (ROM), an optical storage device, a magnetic storage device, a flash memory, a magnetic disk or an optical disk, and / or a combination of the above devices, that is, it can be realized by any type of volatile or non-volatile storage device or a combination thereof.

[0144] The above description is merely the preferred embodiments of the present application, and various modifications or equivalent replacements can be made to the features and embodiments without departing from the spirit and scope of the present application, which should be known by those skilled in the art. In addition, the features and embodiments can be modified to adapt to specific conditions and materials under the teaching of the present application without departing from the spirit and scope of the present application. Therefore, the present application is not limited by the specific embodiments disclosed herein, and all embodiments falling within the scope of the claims of the present application are within the protection scope of the present application.

Claims

1. A monocular vision-based localization and mapping method for pipeline scenarios, characterized in that, The monocular vision-based localization and mapping method is used for simultaneous localization and map construction in the pipeline scene, and includes the following steps: Acquire multiple image frames, and generate multiple first map points based on the multiple image frames; Multiple keyframes are identified in the image frame, and multiple second map points are obtained by feature matching of the multiple keyframes. Extract the initial cylindrical features of the first keyframe, and calculate the cylindrical intersections of multiple keyframes based on the initial cylindrical features to obtain multiple third map points; The pose information of the multiple keyframes and multiple map points are optimized. The optimization includes optimizing the objective function that combines point feature constraints and cylindrical feature constraints. The point feature constraints are the reprojection error constraints of the feature points corresponding to the multiple map points in the keyframes. The cylindrical feature constraints are constraints obtained based on the point-cylinder surface distance error of the initial cylindrical feature. The multiple map points include the first map point, the second map point, and the third map point. Output the final pose information of the optimized keyframes and the final point cloud map including optimized map points.

2. The monocular vision-based localization and mapping method for pipeline scenarios according to claim 1, characterized in that, After calculating the cylindrical intersections of multiple keyframes based on the initial cylindrical features to obtain multiple third map points, the method further includes: Verify the cylindrical feature in the current keyframe; If the verification is successful, the initial cylindrical feature of the first keyframe is used as the initial cylindrical feature of the next keyframe; if the verification fails, the cylindrical feature of the current keyframe is extracted and used as the initial cylindrical feature of the next keyframe.

3. The monocular vision-based localization and mapping method for pipeline scenarios according to claim 2, characterized in that, The verification of the cylindrical feature of the current keyframe includes: calculating the proportion of the cylindrical intersection point to the plurality of map points; if the proportion is less than a first preset threshold, it is defined as verification failure.

4. The monocular vision-based localization and mapping method for pipeline scenarios according to claim 2, characterized in that, After optimizing the pose information of the multiple keyframes and the multiple map points, the method further includes: extracting the cylindrical features of the current keyframe and using them as the initial cylindrical features for the next keyframe.

5. The monocular vision-based localization and mapping method for pipeline scenarios according to claim 2, characterized in that, The extraction of the cylindrical features of the current keyframe includes the following steps: Obtain the initial values ​​of the cylinder parameters for the candidate cylinder; If the number of map points used for cylinder fitting is greater than the second preset threshold, then a cylinder parametric model is constructed. If the cylinder fitting is successful, the cylinder parameter estimation result is output as the cylinder feature of the current keyframe.

6. The monocular vision-based localization and mapping method for pipeline scenarios according to claim 5, characterized in that, The process of obtaining the initial values ​​of the cylinder parameters of the candidate cylinder includes the following steps: Determine whether the current map has candidate cylinders, wherein the current map includes the plurality of map points, and the candidate cylinders are assigned initial values ​​for the cylinder parameters; If the current map does not have the candidate cylinder, then create the candidate cylinder; If the current map has the candidate cylinder, then the cylinder parameter of the candidate cylinder is used as the initial value of the cylinder parameter.

7. The monocular vision-based localization and mapping method for pipeline scenarios according to claim 6, characterized in that, The step of using the cylinder parameters of the candidate cylinder as the initial values ​​of the cylinder parameters includes the following steps: Obtain the transformation relationship between the world coordinate system and the cylindrical coordinate system, wherein the transformation relationship includes the rotation matrix and the translation vector; Based on the transformation relationship, the cylindrical coordinate system is determined and the initial values ​​of the cylindrical parameters are obtained.

8. The monocular vision-based localization and mapping method for pipeline scenarios according to claim 7, characterized in that, The construction of the cylindrical parametric model includes the following steps: The map points in the world coordinate system are transformed to the cylindrical coordinate system. The point-cylindrical distance error function is obtained based on the rotation matrix and the translation vector. The Jacobian matrix is ​​obtained by deriving the point-cylindrical distance error function. A graph optimization model is constructed based on the Jacobian matrix. The graph optimization model is solved to obtain the cylindrical parameter model.

9. A processing device, characterized in that, include: One or more processors; A memory for storing one or more computer programs, and one or more processors for executing the one or more computer programs stored in the memory to cause the one or more processors to perform the monocular vision localization and mapping method for pipeline scenes as described in any one of claims 1-8.

Images