CN115718303A - Dynamic environment-oriented camera and solid-state laser radar fusion repositioning method - Google Patents
Dynamic environment-oriented camera and solid-state laser radar fusion repositioning method Download PDFInfo
- Publication number
- CN115718303A CN115718303A CN202211040009.6A CN202211040009A CN115718303A CN 115718303 A CN115718303 A CN 115718303A CN 202211040009 A CN202211040009 A CN 202211040009A CN 115718303 A CN115718303 A CN 115718303A
- Authority
- CN
- China
- Prior art keywords
- point
- dynamic
- camera
- pose
- points
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Landscapes
- Image Analysis (AREA)
Abstract
A camera and solid-state laser radar fusion relocation method facing a dynamic environment comprises the following steps: obtaining a priori dynamic object through target detection of deep learning YOLOv 5; extracting feature points of the image, calculating the dynamic probability of each feature point in a probability form by combining epipolar geometry and a Bayes formula, eliminating the dynamic feature points, and keeping static feature points; using a DBOW algorithm to represent and update word bags, wherein each image carries the pose calculated by the solid-state laser radar, the pose of the camera is obtained through external parameter calibration, and the point cloud information of the static part of the frame is recorded; and designing a two-stage registration mode to improve the robustness of the system, firstly carrying out similarity calculation on the feature points of the current frame and the previously stored feature points during relocation, and carrying out ICP registration on point cloud back projection in the candidate frame if the feature points are too little matched, so as to further improve the robustness of the system. The present invention combines vision and laser to ameliorate the disadvantages of repositioning in dynamic environments.
Description
Technical Field
The invention relates to the field of global positioning of robots, in particular to a dynamic environment-oriented camera and solid-state laser radar fusion repositioning method.
Background
The global positioning of the robot is a precondition for realizing that the robot can really realize autonomous movement, and the robot can quickly and accurately determine the position of the robot after being started at any time and any place, so that the applicability and the practicability of the robot are effectively improved; on the other hand, the robot can quickly realize the current position of the robot when the positioning is lost, and the capability provides long-term stability and reliability for the positioning of the robot.
In recent thirty years, with the rapid development of computer science and sensors, simultaneous localization and mapping (SLAM) has become an indispensable technology in many fields, such as robotics, autopilot, augmented Reality (AR). Thanks to the development of computer vision, visual SLAM (V-SLAM) has received attention from many researchers and enterprises due to its advantages of low cost, low power consumption, and being able to provide rich landscape information. Although it performs well in static environments, performance degradation and lack of robustness in dynamic environments have been major obstacles to its practical application. For example, patent No. CN110533722A proposes a method and a system for fast repositioning robot based on visual dictionary, patent No. CN110796683A proposes a method for repositioning based on visual feature combined with laser SLAM, where the sensitivity of vision to dynamic objects is much higher than that of a sensor such as laser radar, and it is a low-cost method for implementing repositioning in static environment by using a pure visual method, but when facing real environment, the method will fail, and he will calculate feature points of dynamic objects (such as pedestrians, vehicles, etc.) together, which introduces interference into the system without self or non-self, although patent No. CN110796683A will record the pose of itself in visual dictionary DBow together and design some strategies, it still does not contribute to interference of dynamic objects. The invention patent of patent number CN112596064A proposes a laser and vision integrated indoor robot global positioning method, which simultaneously establishes a laser key frame search library and a vision key frame search library, although the robustness is improved, in a real dynamic environment, the movement of an object can cause the key frame to record wrong feature points, and consequently, the repositioning can be disabled after a large number of feature points are changed.
The existing global positioning method for the robot has the following defects and shortcomings: 1) In the prior art, a visual positioning map and a laser positioning map are respectively established, and then the results of the two global positioning methods are unified by aligning the two maps. However, this method is sensitive to the calibration accuracy of the two sensors, and the resource consumption of separately establishing the two maps is severe. 2) If only the laser radar is used for repositioning, the point cloud registration time is long, and repositioning can be failed if scenes such as structural similarity and no structure occur. At present, some scholars provide an initial pose to enable the robot to perform matching relocation in the range, but the general position of the robot may not be known by people, and the robot cannot realize complete autonomy due to human intervention. 3) In a real environment, the existence of dynamic objects is inevitable, and the influence of relocation on even the whole SLAM system is immeasurable, and the existence of dynamic objects is one of the main problems for preventing the SLAM from further development.
Disclosure of Invention
Aiming at the defects in the prior art, the invention provides a dynamic environment-oriented camera and solid-state laser radar fusion repositioning method, which can realize positioning in a dynamic real environment, effectively combines information of a solid-state laser radar and vision, greatly improves the positioning robustness of a robot in the dynamic environment, and has the advantages of no need of artificial intervention for repositioning, high speed and high precision. Solid state lidar is less costly than rotary lidar and generates dense point clouds but small FOVs.
In order to achieve the purpose, the invention provides the following technical scheme: a dynamic environment-oriented camera and solid-state laser radar fusion repositioning method comprises the following steps:
s1: obtaining a coordinate transformation matrix from a camera to the radar by external reference calibration of a solid state laser radar and the camera
C represents a camera coordinate system, L represents a radar coordinate system, the camera is calibrated, the camera and the radar are time-synchronized, image distortion is removed, and an image after distortion removal is obtained;
s2: identifying dynamic targets in the images by using the pruned YOLOv5, and respectively sending target frames of the obtained dynamic objects into a visual auxiliary module;
the pruning YOLOv5 is specifically as follows:
on the basis of a Yolov5 framework, an L1 regularization layer is applied to a scaling factor in Batch Normalization (BN) for pruning to achieve light weight, so that the method is easy to implement and does not need to change the existing framework.
Equation (1) is a calculation formula of BN (Batch Normalization) layer, where z in And z out Input and output of BN layer, respectively, B is the current smallest mini batch, mu B And σ B Representing the mean and standard deviation of the input activation function on B, γ and β are trainable affine transformation parameters (scale and displacement). Activation size z per number of channels out The sum coefficient γ is positively correlated, and if γ is too small to approach zero, the activation value will also be very small, allowing channels (or neurons) where γ approaches zero to be rejected. However, in normal conditions, after a network is trained, the coefficients of the BN layer are similar to a normal distribution, and γ is distributed in a histogram with epochs in normal training, so that channels near γ are few, and therefore pruning cannot be performed. The value of the BN scaling factor is therefore driven to zero by L1 regularization, enabling identification of irrelevance compactnessThe desired channel (or neuron) because each scaling factor corresponds to a particular convolution channel (or neuron in the fully-connected layer).
The first half of equation (2) is the standard CNN loss function and the second half is the constraint of the added regularization coefficients, where λ is the balance factor, W represents the trainable weights, (x, y) represents the training input and target, λ can be reasonably adjusted from the data set, and g(s) = | s | represents L1 regularization. Due to gradient problems caused by the introduction of the L1 norm, a smooth-L1 substitution can be used.
S3: after extracting the feature points of the image, the visual auxiliary module needs to calculate the dynamic probability of the feature points, and the feature points with larger probability value are regarded as interference points and removed. In order to reduce the influence of the dynamic object on the positioning mapping of the laser SLAM, the point cloud of the current frame is projected on the image, the dynamic object point cloud in the target frame obtained in S2 is clustered, and the dynamic object point cloud is removed in the mapping process.
The above dynamic probability calculation is specifically as follows:
s31: extracting ORB feature points
S311, when the difference between the gray value I (x) of more than N points around the point P of the image and the gray value I (P) of the point P is larger than a threshold value epsilon, the point is considered as a target corner point, and the specific representation is as follows:
s312: the method comprises the following steps of (1) keeping the direction invariance of characteristic points by calculating a mass center, and forming an image pyramid by scaling an original image sequence in a certain proportion so as to keep the scale invariance of the characteristic points: meanwhile, a quadtree uniform algorithm is adopted to ensure that the characteristic points are uniformly distributed in the image
S313: calculating a BRIEF descriptor, and describing information around the feature points through a binary system;
S32：firstly, calculating a basic matrix of epipolar geometry of all points outside a bounding box identified by YOLO, wherein the basic matrix maps feature points in a next frame to a corresponding search domain in a current frame, namely epipolar lines, and assuming that P is a static point, P' is a dynamic point and the distance between the dynamic point and the epipolar lines is D. Let p 1 ,p 2 Respectively represent the matching points of the previous frame and the current frame, and P 1 ,P 2 Is a homogeneous coordinate form, and u, v are pixel coordinates of feature points in the image.
P 1 ＝[u 1 ,v 1 ,1],P 2 ＝[u 2 ,v 2 ,1] (4)
p 1 ＝[u 1 ,v 1 ],p 2 ＝[u 2 ,v 2 ] (5)
The epipolar line is denoted by I, and the formula is calculated:
where X, Y, Z denote epipolar vectors and F is the basis matrix.
The formula (7) shows that a certain paired point falls in the current frame after the last frame is subjected to the base matrix transformation, and the distance between the paired point and the epipolar line I is D. For stable characteristic points, the characteristic points will eventually fall on the epipolar line I after being transformed by the basis matrix in an ideal state, that is, the distance from the point to the epipolar line is zero. Due to the presence of moving objects, each feature point in the image is not strictly constrained to lie on its corresponding epipolar line. Converting the distance information into probability information, and deducing that the probability of movement is higher when the distance between the point and the epipolar line is longer according to the probability information;
s33: the dynamics is a continuous action and is unstable when a frame is processed independently, so by using Bayesian theorem, the idea of evaluating and eliminating dynamic key points by using past and current information is provided. Assume that the initial value of the dynamic probability for each point is ω and that the distance from the matching keypoint to its corresponding epipolar line satisfies a gaussian distribution and that the probability iteration is of a markov nature.
And S331, calculating the moving probability of the key point by using a normal probability density function, wherein the moving probability is defined as:
with a normal distribution, δ represents the standard deviation with a value of 1, desirably zero, where c pi Representing whether the ith point preliminarily screened by the epipolar geometry mode is static or dynamic, and the expression is as follows:
s332, in the invention, the probability propagation formula of one point is as follows:
wherein
The update probability is from the geometric model,
the update probability is within the bounding box from YOLO.
S333, wherein ω is a weight representing the confidence of the bounding box identified for the epipolar geometry model and the YOLO, respectively. ω is calculated as:
wherein N is c Representing geometry of epipolarNumber of outliers, N, calculated by the model s Representing the number of points contained by the bounding box of YOLO. When the probability is larger than omega 1 Then it is determined as a dynamic point and culled out feature point pairs that no longer participate in relocation.
S4: traditional SLAM method for solid-state laser radar is used for constructing environment map and calculating robot pose T wL And the pose of the camera under the world coordinate system is calculated according to the external parameters
S5: continuously acquiring images and screening out key frames in the images by a camera in the moving process, then recording the key frames with the dynamic characteristic points removed by using a DBow algorithm, updating word bags, simultaneously integrating pose information, and storing point clouds outside a dynamic frame projected on the images;
s51: continuously acquiring images and screening out key frames in the images by a camera in the motion process;
and S52, recording the key frames with the dynamic feature points removed by using a DBow algorithm, updating the bag of words, deducing the pose of the camera according to a formula (13) from the pose calculated by using the laser SLAM, and recording the pose, wherein T in the formula (13) wc And T wL Respectively the pose of the camera under the world coordinate system and the pose of the radar under the world coordinate system,
is the camera to radar outer parametric transformation matrix.
S54: projecting the point cloud of the solid-state laser radar of the same frame onto a plane of a camera through internal parameters and external parameters;
s55: and (3) aligning the identification frame calculated by using YOLOv5 with the point cloud projected by the solid-state laser radar on the camera plane, and then extracting and rejecting the dynamic object cluster. Meanwhile, the residual point cloud projected on the image is saved in the form of an image.
S6: when the robot starts to reposition, firstly, the feature point extraction of S3 is carried out, the similarity calculation is carried out by utilizing the constructed word bag and the key frame collected in S5, and if the feature point on the matching is less than the threshold value sigma 1 But greater than the threshold σ 2 And performing point cloud registration in the candidate frame matched in the previous step. Carrying out back projection on the point cloud stored in the candidate frame to form a three-dimensional space point and carrying out ICP registration calculation score on the point cloud and the current point cloud, and finally obtaining the rough pose of the robot with the highest total score;
s61: calculating the DBow vector of the current feature point, and calculating the similarity between the DBow vector of the current feature point and the DBow vector of the historical record;
s62: if the matched points meet the rough attitude estimation, the next step is carried out;
s63: if the matched points are less than the threshold value sigma 1 But greater than the threshold σ 2 Performing back projection on the point cloud recorded in the candidate frame calculated in the step S62 to a three-dimensional space, and performing ICP registration on the point cloud of the current frame in sequence to calculate an optimal candidate frame to obtain a coarse pose of the robot;
s7: and (5) obtaining a coarse pose of the robot through the matching of the S6, and then sending the coarse pose into a solid laser radar preprocessing module to carry out local point cloud matching to obtain an accurate pose of the robot.
According to the method, a priori dynamic object is obtained through target detection of deep learning YOLOv 5; extracting feature points of the image, calculating the dynamic probability of each feature point in a probability form by combining epipolar geometry and a Bayes formula, eliminating the dynamic feature points, and keeping static feature points; using a DBOW algorithm to represent and update word bags, wherein each image carries the pose calculated by the solid-state laser radar, the pose of the camera is obtained through external parameter calibration, and the point cloud information of the static part of the frame is recorded; and designing a two-stage registration mode to improve the robustness of the system, firstly carrying out similarity calculation on the feature points of the current frame and the previously stored feature points during relocation, and carrying out ICP registration on point cloud back projection in the candidate frame if the feature points are too little matched, so as to further improve the robustness of the system. The present invention combines vision and laser to ameliorate the disadvantages of repositioning in dynamic environments.
Compared with the prior art, the invention has the beneficial effects that:
the invention relates to a dynamic environment-oriented camera and solid-state laser radar fusion repositioning method, which solves the problem of repositioning failure of the traditional laser SLAM in a dynamic environment, and also realizes the autonomous repositioning function of a robot without human intervention. Meanwhile, the effect of quick and accurate relocation can be achieved in a real dynamic environment, and the robustness of the traditional SLAM in the dynamic environment is improved.
Drawings
FIG. 1 is a flow chart of the method of the present invention.
Fig. 2 is a pruning flow chart of the present invention.
Fig. 3 is a schematic antipodal geometry for the present invention.
FIG. 4 is a schematic diagram of the relationship between probability and dynamic degree according to the present invention.
Fig. 5 is a schematic diagram illustrating the effect of the invention after the dynamic feature points are removed.
FIG. 6 is a schematic diagram of the effect of dynamic feature point elimination on the data set of Munich university of industry.
FIG. 7 is a schematic pre-relocation diagram of the present invention.
FIG. 8 is a schematic post-relocation diagram of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention is further described in detail below with reference to the accompanying drawings. It should be understood that the description of the embodiments is only for the purpose of illustrating the present invention and is not intended to limit the present invention.
The invention relates to a flow chart of a camera and solid-state laser radar fusion relocation method facing a dynamic environment, which comprises the following steps as shown in figure 1.
S1: the external reference calibration of the solid-state laser radar and the Camera is to obtain a transformation matrix from a Camera coordinate system to a radar coordinate system by using Livox Camera Calib algorithm of hong Kong university
Calibrating the camera by using calibration software carried by Realsense d455, synchronizing the time of the camera and the radar, removing image distortion, and obtaining an image after distortion removal;
s2: as shown in fig. 7, the YOLOv5 pruning flowchart identifies dynamic targets in the image by using the pruned YOLOv5, and respectively sends the obtained target frames of the dynamic objects into the visual auxiliary module;
s3: after the visual auxiliary module extracts the feature points of the image, the dynamic probability calculation needs to be carried out on the feature points, and the probability value is larger than omega 1 The one that =0.75 is considered as the interference point and rejects him. In order to reduce the influence of dynamic objects on positioning mapping of the laser SLAM, the point cloud of the current frame is projected on an image, the dynamic object point cloud in the target frame obtained in the previous step is clustered, and the dynamic object point cloud is removed in the mapping process. Experiments on the data set were done and on the own data set, as shown in fig. 4, this is an effect on the own data set, where the green dots are stable static dots. This is shown in fig. 5, which is verified on a data set of the famous university of munich industry, and it can be seen that the form of probability retains the points in the YOLO target box, further improving the robustness of the system, and the red points are shown on the graph as the characteristic points retained in the box, and the green points are the stable characteristic points outside the box.
S31: firstly, putting characteristic points outside a YOLOv5 frame into a set P, then utilizing points in the set P to match with a previous frame to obtain corresponding matching points, putting the corresponding matching points into an M set, and carrying out epipolar geometric constraint calculation on matching points of the M set and the P set to obtain an F matrix;
s32: calculating the distances D from all the characteristic points to the epipolar line by using a formula (7);
s33, assuming that the distance from the point to the epipolar line is in accordance with standard normal distribution, substituting D calculated in the S32 into the formula (8) to calculate the set dynamic probability of the point;
s34, calculating omega weight, which represents confidence of the bounding box of the epipolar geometry model and the YOLO identification respectively. ω is calculated as:
wherein N is c Representing the number of outliers, N, calculated from an epipolar geometric model s Representing the number of points contained by the bounding box of YOLO.
S35: evaluating and eliminating dynamic key points by using past and current information to further strengthen the effectiveness of screening the dynamic points, so that if the geometric probability of each point and the initial value of the probability of the point in a YOLOv5 box are assumed to be omega =0.5, probability iteration is carried out by using an equation (10) if the obtained probability is more than omega 1 =0.75, it is added to the set of dynamic feature points S
S4: traditional SLAM method for solid-state laser radar is used for constructing environment map and calculating robot pose T wL And the pose of the camera under the world coordinate system is calculated according to the external parameters
S5: continuously acquiring images and screening key frames in the images by a camera in the moving process, recording the key frames with the dynamic characteristic points removed by using a DBow algorithm, updating word bags, simultaneously integrating the pose information calculated in the S4, and storing point clouds outside the dynamic frames projected on the images;
s51: continuously acquiring images and screening out key frames in the images by a camera in the moving process;
s52, recording the key frames with the dynamic feature points removed by using a DBow algorithm, updating word bags, deducing the pose of the camera according to a formula (13) from the pose calculated by using the laser SLAM, and recording the pose, wherein T in the formula (13) wc And T wL Respectively the pose of the camera under the world coordinate system and the pose of the radar under the world coordinate system,
is the camera to radar outer parametric transformation matrix.
S54: projecting the point cloud of the solid laser radar of the same frame onto the plane of the camera through internal parameters and external parameters;
s55: and (3) projecting the calculated identification frame by using YOLOv5 and the solid-state laser radar to a point cloud on a camera plane for alignment, and then extracting and rejecting the dynamic object cluster. Meanwhile, the residual point cloud projected on the image is saved in the form of an image.
S6: when the robot starts to reposition, firstly, the feature point extraction of S3 is carried out, the similarity calculation is carried out by utilizing the constructed word bag and the key frame collected in S5, and if the feature point on the feature point matching is less than the threshold value sigma 1 =500 but greater than threshold σ 2 =200, performing ICP pairing calculation on the candidate frame matched with the current characteristic point and the stored point cloud after back projection to form a three-dimensional space point and the current point cloud, and finally obtaining the rough pose of the robot with the highest total score;
s61: calculating the DBow vector of the current characteristic point, and calculating the similarity between the DBow vector of the current characteristic point and the DBow vector of the historical record;
s62: if the matched points meet the rough attitude estimation, the next step is carried out;
s63: if the matched points are less than the threshold value sigma 1 =500 but greater than threshold σ 2 =200, the point clouds recorded in the candidate frame calculated in the step S62 are back-projected into a three-dimensional space, and the point clouds of the current frame are sequentially subjected to ICP registration to calculate the optimal point cloudObtaining the rough pose of the robot by the candidate frames;
s7: and (5) obtaining a coarse pose of the robot through the matching of the S6, and then sending the coarse pose into a solid laser radar preprocessing module to carry out local point cloud matching to obtain an accurate pose of the robot.
Fig. 6 is a comparison between fig. 6 and fig. 7, which is for verifying the effect and speed of repositioning, and fig. 6 is a view of the red dots on the right side before being repositioned, which shows that they do not overlap the white map dots, and fig. 7 shows that the point cloud of the red current frame has successfully overlapped the white map dots. It is noted that the Time of Wall Time below the two figures may find the speed of relocation to be only 80ms to complete the relocation. And the map is formed after being interfered by dynamic objects, the relocation can still be completed, which fully shows the reliability and the practicability of the invention.
It should be emphasized that the embodiments described herein are illustrative and not restrictive, and thus the present invention includes, but is not limited to, the embodiments described in the detailed description, and that other embodiments similar to those described herein may be made by those skilled in the art without departing from the scope of the present invention.
Claims (7)
1. A camera and solid-state laser radar fusion relocation method facing a dynamic environment is characterized by comprising the following steps:
s1: obtaining a coordinate transformation matrix from a camera to the radar by external reference calibration of a solid state laser radar and the camera
C represents a camera coordinate system, L represents a radar coordinate system, the camera is calibrated, the time of the camera is synchronous with that of the radar, image distortion is removed, and an image after distortion removal is obtained;
s2: identifying dynamic targets in the images by using the pruned YOLOv5, and respectively sending target frames of the obtained dynamic objects into a visual auxiliary module;
s3: after extracting the feature points of the image, the visual auxiliary module needs to calculate the dynamic probability of the feature points, and takes the feature points with larger probability values as interference points and eliminates the interference points; in order to reduce the influence of a dynamic object on positioning mapping of the laser SLAM, the point cloud of the current frame is projected on an image, the dynamic object point cloud in the target frame obtained in S2 is clustered, and the dynamic object point cloud is removed in the mapping process;
s4: traditional SLAM method is used for solid-state laser radar to construct environment map and calculate robot pose T wL And the pose of the camera under the world coordinate system is calculated according to the external parameters
S5: continuously acquiring images and screening out key frames in the images by a camera in the moving process, then recording the key frames with the dynamic characteristic points removed by using a DBow algorithm, updating word bags, simultaneously integrating pose information, and storing point clouds outside a dynamic frame projected on the images;
s6: when the robot starts to reposition, firstly, feature point extraction of S3 is carried out, similarity calculation is carried out by using the constructed word bag and the key frame collected in S5, and if the feature point on matching is less than the threshold value sigma 1 But greater than the threshold σ 2 Point cloud registration is carried out in the candidate frame matched in the previous step; carrying out ICP (inductively coupled plasma) pairing calculation score on the point cloud stored in the candidate frame by back projection to form a three-dimensional space point and the current point cloud, and finally obtaining the rough pose of the robot with the highest total score;
s7: and (5) obtaining a coarse pose of the robot through the matching of the S6, and then sending the coarse pose into a solid laser radar preprocessing module to carry out local point cloud matching to obtain an accurate pose of the robot.
2. The dynamic environment-oriented camera and solid-state lidar fusion relocation method of claim 1, wherein: the pruning YOLOv5 in S2 is specifically as follows:
on the basis of a YOLOV5 framework, an L1 regularization layer is applied to a scaling factor in Batch Normalization (BN) for pruning to achieve light weight, so that the method is easy to realize and does not need to change the existing framework;
equation (1) is a calculation formula of BN (Batch Normalization) layer, where z in And z out Input and output of BN layer, B is the current smallest mini batch, mu B And σ B Represents the mean and standard deviation of the input activation function on B, γ and β are trainable affine transformation parameters (scale and displacement); activation size z per number of channels out The sum coefficient γ is positively correlated, if γ is too small close to zero, the activation value will also be very small, allowing channels (or neurons) where γ approaches zero to be rejected; but under normal conditions, after a network is trained, the coefficients of the BN layer are similar to normal distribution, and gamma is distributed in a histogram along with epochs during normal training, so that channels near which gamma approaches zero are few, and pruning cannot be performed; the value of the BN scaling factors is therefore pushed to zero by L1 regularization, enabling identification of insignificant channels (or neurons), since each scaling factor corresponds to a particular convolutional channel (or neuron in a fully-connected layer);
the first half of equation (2) is the standard CNN loss function and the second half is the constraint of the added regularization coefficients, where λ is the balancing factor, W represents the trainable weights, L (x, y) represents the training input and the target, λ can be reasonably adjusted according to the data set, and g(s) = | s | is for L1 regularization; due to gradient problems caused by the introduction of the L1 norm, a smooth-L1 substitution can be used.
3. The dynamic environment-oriented camera and solid-state lidar fusion relocation method of claim 1, wherein: s3 specifically comprises the following steps:
s31: extracting ORB feature points
S311, when the difference between the gray value I (x) of the points of the image, which are more than N, around the point P and the gray value I (P) of the point P is more than a threshold value epsilon, the point is regarded as a target corner point, and the specific expression is as follows:
s312: keeping the direction invariance of the characteristic points by calculating the mass center, and forming an image pyramid by scaling an original image sequence in a certain proportion to keep the scale invariance of the characteristic points: meanwhile, a quadtree uniform algorithm is adopted to ensure that the characteristic points are uniformly distributed in the image
S313: calculating a BRIEF descriptor, and describing information around the feature points through a binary system;
s32: firstly, calculating a basic matrix of epipolar geometry of all points outside a bounding box identified by YOLO, wherein the basic matrix maps characteristic points in a next frame to a corresponding search domain in a current frame, namely epipolar lines, and supposing that P is a static point, P' is a dynamic point and the distance between the dynamic point and the epipolar lines is D; let p 1 ,p 2 Respectively represent the matching points of the previous frame and the current frame, and P 1 ,P 2 Is a homogeneous coordinate form, u, v are pixel coordinates of feature points in the image;
P 1 ＝[u 1 ,v 1 ,1],P 2 ＝[u 2 ,v 2 ,1] (4)
p 1 ＝[u 1 ,v 1 ],p 2 ＝[u 2 ,v 2 ] (5)
the epipolar line is denoted by I, and the formula is calculated:
wherein X, Y, Z represent epipolar vectors, and F is a basis matrix;
formula (7) shows that a certain matching point falls in the current frame after the last frame is subjected to basic matrix transformation, and the distance between the matching point and the epipolar line I is D; for stable characteristic points, the characteristic points finally fall on an epipolar line I after being transformed by a basic matrix in an ideal state, namely the distance from the points to the epipolar line is zero; due to the existence of moving objects, each feature point in the image is not strictly restricted to be positioned on the corresponding epipolar line; converting the distance information into probability information, and deducing that the probability of movement is higher when the distance between the point and the epipolar line is longer according to the probability information;
s33: the dynamics is a continuous action and is unstable when a frame is processed independently, so the Bayesian theorem is used, and the dynamic key point estimation and elimination method adopts the past and current information; assuming that the initial value of the dynamic probability of each point is ω and the distance from the matched keypoint to its corresponding epipolar line satisfies a gaussian distribution and the probabilistic iteration is in accordance with the markov property;
and S331, calculating the moving probability of the key point by using a normal probability density function, wherein the moving probability is defined as:
with a normal distribution, δ represents the standard deviation with a value of 1, desirably zero, where c pi Representing whether the ith point preliminarily screened by the epipolar geometry mode is static or dynamic, and the expression is as follows:
s332, in the invention, the probability propagation formula of one point is as follows:
wherein
The update probability is from the geometric model,
the update probability is within the bounding box from YOLO;
s333, wherein omega is weight representing confidence of the boundary box recognized by the epipolar geometric model and the YOLO respectively; ω is calculated as:
wherein N is c Representing the number of outliers, N, calculated from an epipolar geometric model s Represents the number of points contained by the bounding box of YOLO; when the probability is larger than omega 1 It is determined as a dynamic point and culled out feature point pairs that no longer participate in the relocation.
4. The dynamic environment-oriented camera and solid-state lidar fusion relocation method of claim 1, wherein: traditional SLAM method for solid-state laser radar is used for constructing environment map and calculating robot pose T wL And the pose of the camera under the world coordinate system is calculated according to the external parameters
5. The dynamic environment-oriented camera and solid-state lidar fusion relocation method of claim 1, wherein: the information storage mode of the camera and the radar in the step S5 is as follows:
s51: continuously acquiring images and screening out key frames in the images by a camera in the motion process;
s52, recording the key frames with the dynamic feature points removed by using a DBow algorithm, updating word bags, deducing the pose of the camera according to a formula (13) from the pose calculated by using the laser SLAM, and recording the pose, wherein T in the formula (13) wc And T wL Respectively the pose of the camera under the world coordinate system and the pose of the radar under the world coordinate system,
is the camera to radar outer parametric transformation matrix;
s54: projecting the point cloud of the solid-state laser radar of the same frame onto a plane of a camera through internal parameters and external parameters;
s55: projecting the identification frame calculated by using YOLOv5 and the solid-state laser radar to a point cloud on a camera plane for alignment, and then extracting and removing the people cluster; meanwhile, the residual point cloud projected on the image is saved in the form of an image.
6. The dynamic environment-oriented camera and solid-state lidar fusion relocation method of claim 1, wherein: the relocation matching in S6 is specifically as follows:
s61: calculating the DBow vector of the current characteristic point, and calculating the similarity between the DBow vector of the current characteristic point and the DBow vector of the historical record;
s62: if the matched points meet the rough attitude estimation, the next step is carried out;
s63: if the matched points are less than the threshold value sigma 1 But greater than the threshold σ 2 The point cloud recorded in the candidate frame calculated in S62 is back-projected into the three-dimensional space, and the point cloud of the current frame is sequentially subjected to ICP registration to calculate an optimal candidate frame, so as to obtain the coarse pose of the robot.
7. The dynamic environment-oriented camera and solid-state lidar fusion repositioning method of claim 1, wherein repositioning is specifically: and S6, obtaining a coarse pose of the robot through matching, and then sending the coarse pose into a solid laser radar preprocessing module for local point cloud matching to obtain an accurate pose of the robot.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211040009.6A CN115718303A (en) | 2022-08-29 | 2022-08-29 | Dynamic environment-oriented camera and solid-state laser radar fusion repositioning method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211040009.6A CN115718303A (en) | 2022-08-29 | 2022-08-29 | Dynamic environment-oriented camera and solid-state laser radar fusion repositioning method |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115718303A true CN115718303A (en) | 2023-02-28 |
Family
ID=85254854
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211040009.6A Pending CN115718303A (en) | 2022-08-29 | 2022-08-29 | Dynamic environment-oriented camera and solid-state laser radar fusion repositioning method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115718303A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117911271A (en) * | 2024-01-22 | 2024-04-19 | 哈尔滨工业大学（威海） | Dynamic obstacle rapid point cloud removing method and system based on YOLO |
-
2022
- 2022-08-29 CN CN202211040009.6A patent/CN115718303A/en active Pending
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117911271A (en) * | 2024-01-22 | 2024-04-19 | 哈尔滨工业大学（威海） | Dynamic obstacle rapid point cloud removing method and system based on YOLO |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111798475B (en) | Indoor environment 3D semantic map construction method based on point cloud deep learning | |
| CN112258618B (en) | Semantic mapping and positioning method based on fusion of prior laser point cloud and depth map | |
| CN110070615B (en) | Multi-camera cooperation-based panoramic vision SLAM method | |
| CN110533722B (en) | Robot rapid repositioning method and system based on visual dictionary | |
| CN113345018B (en) | Laser monocular vision fusion positioning mapping method in dynamic scene | |
| CN110675418B (en) | Target track optimization method based on DS evidence theory | |
| CN110097553A (en) | The semanteme for building figure and three-dimensional semantic segmentation based on instant positioning builds drawing system | |
| CN111201451A (en) | Method and device for detecting object in scene based on laser data and radar data of scene | |
| CN110827415A (en) | All-weather unknown environment unmanned autonomous working platform | |
| CN112784873B (en) | Semantic map construction method and device | |
| Mu et al. | Research on SLAM algorithm of mobile robot based on the fusion of 2D LiDAR and depth camera | |
| US12087028B2 (en) | Lifted semantic graph embedding for omnidirectional place recognition | |
| CN111998862B (en) | BNN-based dense binocular SLAM method | |
| CN114140527B (en) | Dynamic environment binocular vision SLAM method based on semantic segmentation | |
| CN114088081B (en) | Map construction method for accurate positioning based on multistage joint optimization | |
| Yin et al. | Cae-lo: Lidar odometry leveraging fully unsupervised convolutional auto-encoder for interest point detection and feature description | |
| CN117197333A (en) | Space target reconstruction and pose estimation method and system based on multi-view vision | |
| CN114137564A (en) | Automatic indoor object identification and positioning method and device | |
| CN114140539A (en) | Method and device for acquiring position of indoor object | |
| CN116772820A (en) | Local refinement mapping system and method based on SLAM and semantic segmentation | |
| CN115718303A (en) | Dynamic environment-oriented camera and solid-state laser radar fusion repositioning method | |
| Zhang et al. | Improved feature point extraction method of ORB-SLAM2 dense map | |
| CN117253003A (en) | Indoor RGB-D SLAM method integrating direct method and point-plane characteristic method | |
| Bojanić et al. | A review of rigid 3D registration methods | |
| Friedel | Event-based visual-inertial odometry using smart features |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |