CN113108771A - Movement pose estimation method based on closed-loop direct sparse visual odometer - Google Patents
Movement pose estimation method based on closed-loop direct sparse visual odometer Download PDFInfo
- Publication number
- CN113108771A CN113108771A CN202110245806.7A CN202110245806A CN113108771A CN 113108771 A CN113108771 A CN 113108771A CN 202110245806 A CN202110245806 A CN 202110245806A CN 113108771 A CN113108771 A CN 113108771A
- Authority
- CN
- China
- Prior art keywords
- frame
- pose
- point
- points
- frames
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 75
- 230000000007 visual effect Effects 0.000 title claims abstract description 25
- 238000005457 optimization Methods 0.000 claims abstract description 22
- 238000001514 detection method Methods 0.000 claims abstract description 13
- 230000004913 activation Effects 0.000 claims abstract description 10
- 238000013519 translation Methods 0.000 claims abstract description 9
- 230000002093 peripheral effect Effects 0.000 claims abstract description 4
- 230000009466 transformation Effects 0.000 claims description 21
- 238000012937 correction Methods 0.000 claims description 13
- 239000011159 matrix material Substances 0.000 claims description 9
- 230000003287 optical effect Effects 0.000 claims description 6
- 230000036544 posture Effects 0.000 claims description 6
- 238000005259 measurement Methods 0.000 claims description 4
- 238000004364 calculation method Methods 0.000 description 5
- 238000013507 mapping Methods 0.000 description 5
- 238000011160 research Methods 0.000 description 3
- 238000002474 experimental method Methods 0.000 description 2
- 238000000605 extraction Methods 0.000 description 2
- 241000152447 Hades Species 0.000 description 1
- 230000003190 augmentative effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 125000004122 cyclic group Chemical group 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Image Analysis (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Manipulator (AREA)
Abstract
The invention discloses a closed-loop direct sparse vision odometer-based mobile pose estimation method, which comprises the following steps of: acquiring a peripheral image sequence of the robot; initializing the front end of the visual odometer on the basis of the first two frames of the image sequence, and determining a reference frame; the front end of the visual odometer solves the relative pose change among image sequence frames by a direct method based on the minimized luminosity error and judges whether the image sequence frames are key frames or not; adding the new key frame into the rear end of the visual odometer for optimization, and simultaneously combining all key frame poses and activation points to optimize a total luminosity error function; and performing marginalization processing on the old key frame, performing closed-loop detection based on the global pose graph, and determining the optimal motion pose estimation among frames. The invention can enable the robot to accurately and rapidly resolve the motion pose in a complex environment, obviously reduce the accumulated errors of rotation, translation, scale drift and the like through pose graph optimization, and has the advantages of less time consumption, better real-time property and stronger robustness in the area lacking the corner points.
Description
Technical Field
The invention relates to the technical field of machine vision and instant positioning, in particular to a mobile pose estimation method based on a closed-loop direct sparse vision odometer.
Background
The research field of the mobile robot comprises a plurality of sub-problems, wherein the positioning and mapping problem is a key problem which needs to be solved for realizing the autonomous movement and obstacle avoidance function of the robot. The robot can rely on information provided by a Global Positioning System (GPS) to a great extent in outdoor motion, but is limited by the accuracy and coverage of the GPS, positioning and Mapping of indoor scenes often depends on a Simultaneous positioning and Mapping (SLAM) algorithm, and since the 20 th century and 80 th century, an instant positioning and Mapping (SLAM) system has become an active research field in computer vision and robotics, and the SLAM system has become a basic module for many applications requiring real-time positioning, such as mobile robots, automatic driving, virtual and augmented reality. Visual SLAM is popular in part because cameras are readily available in consumer products and passively acquire rich information about the environment.
The SLAM system consists of a visual odometer front end and a back end optimization, the back end creates and maintains key frame mapping, and global drift is reduced through cyclic closed loop detection and map optimization. The front end can solve the interframe motion pose according to the current video frame overall situation, and the visual odometer pose solving method can be divided into two types: indirect (feature-based) methods, minimizing reprojection errors by the pairwise relationship between feature points between frames, and direct methods, jointly estimating pose by minimizing photometric errors between frames.
Although feature-based methods have long been the mainstay, recent advances in direct methods have shown greater accuracy and robustness, particularly when the image does not contain sufficient well-defined corner features. Whereas the closed-loop detection generally uses the traditional feature-based Bag-of-word (Bow).
At present, most of visual inertial odometers are based on a characteristic point method, and have the following problems:
the feature point method needs to extract a large number of feature points in each image, perform feature point matching between adjacent image frames, and perform pose estimation according to the matched feature points. Because a large number of feature points are extracted and matched in each frame, the calculation amount of the system is large, and certain influence is exerted on the real-time performance. And the method is very dependent on environmental information, and can hardly work when the acquired image has no good texture features.
Compared with the characteristic point method, the sparse direct method applied by the invention saves the time for subsequent characteristic point extraction and characteristic point matching, and has better real-time performance. The method has better performance in weak texture regions lacking corner points, and meanwhile, the accumulated errors of rotation, translation, scale drift and the like are remarkably reduced by applying closed-loop detection.
Disclosure of Invention
The invention aims to provide a mobile pose estimation method based on a closed-loop direct sparse visual odometer on the premise of saving cost and ensuring reliability, and provides a method for realizing accurate and rapid pose resolving of a robot in a complex environment by applying a direct visual odometer calculation method to perform indoor robot visual positioning.
The invention is realized by at least one of the following technical schemes.
A mobile pose estimation method based on a closed-loop direct sparse visual odometer comprises the following steps:
s1, acquiring a robot peripheral image sequence;
step S2, initializing based on the first two frames of images, and constructing a reference frame of a subsequent image sequence;
s3, acquiring relative pose change between frames and judging whether the pose change is a key frame;
step S4, adding a rear-end sliding window into the new key frame, and jointly constructing a total luminosity error function by the positions and postures of all the key frames in the window and the key points extracted from the key frames, and optimizing the initial value of the relative position and posture in front-end estimation;
and step S5, marginalizing the old key frames, constructing a global pose graph based on descriptors of all the key frames for closed-loop detection, and estimating the pose of the robot.
Preferably, step S2 is further to initialize the input of the camera sensor at the front end of the odometer; the initialization needs two frames of images, the first frame is used as a reference frame, namely a Gaussian pyramid is constructed for the images according to a world coordinate system of a camera coordinate system where the first frame is located, gradient histograms of all pixel points in the 0 th layer of the image pyramid are counted, different gradient points are extracted from the images layer by taking the number of bits as a gradient threshold, a Nearest neighbor (Fast Library for Approximate neighbor Neighbors) index is constructed for each gradient point, s points closest to the s gradient points are found in the same layer to construct a neighborhood relation, a point with the minimum distance to the current point is found in the Nearest neighbor index of the next layer and is set as an associated point, and the current layer and the next layer are associated, so that the spread of the optical flow pyramid and the inverse depth value is facilitated.
Preferably, in step S2, let the second frame be IvThe first frame is Iu,IvBased on IuAnd constructing an interframe luminosity error function by using the selected key point with the maximum gradient:
Eu,v,w=ωp(Iv[p`]-avuIu[p]-bvu)
wherein ω ispTo compensate for the weighting parameter, p is IuPoint of medium gradient, p' denotes p is in the current frame IvBack-projected coordinates of (1)vuCorrecting the parameters for the exposure time, bvuCorrecting parameters for the image brightness basis;
iteratively minimizing the photometric error by a gauss-newton method; if the error is optimized and not converged, judging that the initialization fails, resetting the first frame and repeating the process; otherwise, the initialization is successful, the first frame is set as a key frame, the gradient points extracted from the first frame are activated, and the gradient points are added into the optimization of the sliding window; the second frame enters sliding window optimization after initialization.
Preferably, the gradient point activation is divided into an activation point and an immature point, the immature point being a point extracted in the image or not converged in depth; and the activated points are continuously filtered among frames through a depth filter, so that the depth of the points is continuously converged, and finally the obtained points are activated and added into a sliding window.
Preferably, step S3 is further to perform pose solution on each input frame of new image based on the latest key frame, that is, project the gradient points of the latest key frame into the current frame to construct a photometric error function:
Eu,v,w=ωp(Iv[p`]-avuIu[p]-bvu);
wherein ω ispTo compensate for the weighting parameter, p is IuPoint of medium gradient, p' denotes p in the current frame IvBack-projected coordinates of (1)vuCorrecting the parameters for the exposure time, bvuCorrecting parameters for the image brightness basis; and iteratively minimizing the luminosity error through a Gauss-Newton method, and if the error is optimized and converged, taking the new frame as a key frame and adding the key frame into a sliding window for optimization.
Preferably, in step S4, each point is projected into all key frames in the inter-frame pose solution by using a sparse direct method or a direct optical flow matching method, a residual error in each frame is calculated, a sliding window composed of a plurality of system key frames is used at the back end, all key frame poses and activation points in the sliding window are all combined together to optimize a total luminosity error function, and when a front-end line judges that a frame meets a condition and is inserted into the back end, a minimum luminosity error to be optimized is defined as follows:
let the image frame set S ═ T1,T2…Tj,p1,p2…pkIn which T isjRepresenting the pose, p, of the keyframe jkRepresenting coordinates of pixel points in a keyframe, minimizing photometric errorsThe method comprises the following steps:
wherein,tu、tvas an image Iu、IvA corresponding exposure time;is a collection of pixel points, pwIs a pixel point in the set, p is IvMiddle pixel point, avIs IvExposure time correction parameter of auIs IuExposure time correction parameter of (b)vIs IvLuminance floor correction parameter, buIs IuA luminance floor correction parameter; omegapTo compensate for the weighting parameter; p' is a constant value containing an inverse depth parameter dpIn the image I at the pixel coordinate pvThe reprojection coordinates of (c).
Preferably, the reprojection coordinate p' is formulated as follows:
p`=Π(RΠ-1(p,dp)+t)
r, T is a rotation matrix and a translation vector, and is transformed by a transformation matrix Tu、TvCalculated result, pi and pi-1Representing the reprojection and inverse reprojection transforms, respectively.
Preferably, in step S5, an old frame and an old point are removed by using a marginalization strategy, and if a point is not in the camera view, the point is marginalized; if the number of frames in the sliding window exceeds a set threshold value, selecting one frame for marginalization; when a frame is marginalized, the map points that dominate it will be removed.
Preferably, step S5 is to perform closed-loop detection using pose graph optimization, add global pose in the sliding window to maintain the connection between key frames, take the relative pose transformation of key frames in the sliding window as the measurement data of pose matching, based on the matching method that the bag-of-words method combines the ORB features with the original point selection policy, after selecting the loop matching sub, calculate the similar transformation of matching sub to the current key frame, and add it to the global pose graph, and estimate the pose of the camera along with optimization.
Preferably, the matching points are solved by similarity transformation (similarity transformation) using h, and then solvedRotation matrix, translation vector and scale between two coordinate systems, i.e. sim (h) transformation, which transforms the similarity of matching sub-pairs to the current key-frame into ScrThen S iscrBy minimizing a cost function ElObtaining:
El=ω(ScrΠ-1(p,dp)-Π-1(q,dq))
wherein p and q represent matched pixel points in the matched sub-and key-frames, dp、dqRepresenting the inverse depth corresponding to the pixel points p and q, and omega is a balance weight parameter.
Compared with the prior art, the invention has the beneficial effects that:
the sparse direct method applied by the invention saves the time for subsequent feature point extraction and feature point matching, and has better real-time performance. The method has better performance in weak texture regions lacking corner points, and meanwhile, the accumulated errors of rotation, translation, scale drift and the like are remarkably reduced by applying closed-loop detection. The real-time performance is better, and the robustness is stronger in the area lacking the corner points.
Drawings
FIG. 1 is a flow chart of a method for estimating a mobile pose based on a closed-loop direct sparse visual odometer according to the present embodiment;
fig. 2 is a closed-loop detection framework diagram of the present embodiment.
Detailed Description
The invention is further described below with reference to the accompanying drawings. It should be noted that the following description gives detailed embodiments and specific operation procedures, and is intended to describe the present application in detail, but the scope of the present invention is not limited to this embodiment.
As shown in fig. 1, a flow chart of a method for estimating a motion pose based on a closed-loop direct sparse visual odometer includes the following steps:
s1, acquiring a robot peripheral image sequence through a Kinect camera;
in order to further verify the effectiveness and accuracy of the invention, the scheme of the invention is transplanted to a mobile robot platform Turtlens and a Kinect camera sensor is carried. Field experiments were performed in an indoor corridor environment.
As another example, the Kinect camera may be replaced with a small-foraging smart camera depth plate as a vision sensor.
Step S2, initializing based on the first two frames of images, and constructing a reference frame of a subsequent image sequence;
the method comprises the steps that firstly, input of a camera sensor needs to be initialized, two frames of images are needed in an initialization stage, the first frame is used as a reference frame, namely a Gaussian pyramid is constructed for the images according to a world coordinate system of a camera coordinate system where the first frame is located, gradient histograms of all pixel points in a 0 th layer of the image pyramid are counted, the number of bits is used as a gradient threshold value, points with larger gradient are extracted from the images layer by layer, a Nearest neighbor (Fast Library for application neighbor Neighbors) index is constructed for each gradient point, and 10 points closest to the gradient point are found in the same layer to construct a neighborhood relation.
As another embodiment, 20 points closest to the points can be found to construct a neighborhood relationship, so that the neighborhood range is improved, and the matching accuracy is improved.
Finding a point with the minimum distance from the current point in the nearest neighbor index of the next layer, and setting the point as an associated point, wherein the aim is to associate the current layer with the next layer, so that the propagation of an optical flow pyramid and an inverse depth value is facilitated;
let the second frame be IvThe first frame is Iu,IvBased on IuAnd constructing an interframe luminosity error function by using the selected key points with larger gradients:
Eu,v,w=ωp(Iv[p`]-avuIu[p]-bvu)
wherein ω ispTo compensate for the weighting parameter, p is IuThe point with larger gradient in the middle, p' represents that p is in the current frame IvBack-projected coordinates of (1)vuCorrecting the parameters for the exposure time, bvuCorrecting parameters for the image brightness basis;
iteratively minimizing the photometric error by a gauss-newton method; if the error is optimized and not converged, judging that the initialization fails, resetting the first frame and repeating the process; otherwise, the initialization is successful, the first frame is set as a key frame, the gradient points extracted from the first frame are activated, and the gradient points are added into the optimization of the sliding window; the second frame enters sliding window optimization after initialization.
In the invention, the pixel points are divided into two types, one is an activation point and the other is an immature point. Often the points that have just been extracted or have unconverged depths in the image are called immature points, with a very large depth range. The depth filter continuously filters between frames, so that the depth of the point is continuously converged, and the finally obtained point is activated and added into the sliding window, and the point is called an activation point.
S3, resolving the relative pose change between frames based on the direct method of the minimum luminosity error and judging whether the frames are key frames;
similar to the process of establishing inter-frame associated pixel points by minimizing luminosity errors in the initialization process of a first frame and a second frame, the algorithm carries out pose calculation on each input frame of new images based on the nearest key frame, namely, the gradient points of the nearest key frame are projected to the current frame to establish a luminosity error function:
Eu,v,w=ωp(Iv[p`]-avuIu[p]-bvu)
wherein ω ispTo compensate for the weighting parameter, p is IuThe point with larger gradient in the middle, p' represents that p is in the current frame IvBack-projected coordinates of (1)vuCorrecting the parameters for the exposure time, bvuCorrecting parameters for the image brightness basis; and iteratively minimizing the luminosity error through a Gauss-Newton method, and if the error is optimized and converged, taking the new frame as a key frame and adding the key frame into a sliding window for optimization.
Step S4, adding a rear-end sliding window into the new key frame, and jointly constructing a total luminosity error function by the positions and postures of all the key frames in the window and the key points extracted from the key frames, and optimizing the initial value of the relative position and posture in front-end estimation;
projecting each point to all key frames in the interframe pose calculation by using a sparse direct method, calculating residual errors in each frame, using a sliding window consisting of a plurality of system key frames at the rear end, and optimizing all key frame poses and activation points in the sliding window into a total luminosity error function in a combined manner, wherein when a front end process judges that a certain frame meets conditions and is inserted into the rear end, the minimum luminosity error to be optimized is defined as follows:
let the image frame set S ═ T1,T2…Tj,p1,p2…pkIn which T isjRepresenting the pose, p, of the keyframe jkRepresenting coordinates of pixel points in a keyframe, minimizing photometric errorsThe method comprises the following steps:
wherein, tu、tvAs an image Iu、IvA corresponding exposure time;is a collection of pixel points, pwIs a pixel point in the set, p is IvMiddle pixel point, avIs IvExposure time correction parameter of auIs IuExposure time correction parameter of (b)vIs IvLuminance floor correction parameter, buIs IuLuminance floor correction parameter, ωpTo compensate for the weighting parameter; p' is a depth parameter d containing the inversepIn the image I at the pixel coordinate pvThe formula p' is as follows:
p`=Π(RΠ-1(p,dp)+t)
r, T is a rotation matrix and a translation vector, and is transformed by a transformation matrix Tu、TvCalculated result, pi and pi-1Representing the reprojection and inverse reprojection transforms, respectively. Compared with the constraint between two frames, more constraint conditions are provided in the sliding window, and the initial value of the relative pose estimated by the front end can be optimized.
As another example, the sparse direct method may be replaced with a direct optical flow matching method.
And step S5, marginalizing the old key frames, and constructing a global pose graph based on the descriptors of all the key frames to perform closed-loop detection. Removing the old frame and the old point by adopting an marginalization strategy, and if one point is not in the camera view, marginalizing the point; if the number of frames in the sliding window exceeds a set threshold value, selecting one frame for marginalization; when a frame is marginalized, the map points that dominate it will be removed.
Meanwhile, closed-loop detection is carried out by using pose graph optimization, global pose maintaining key frame connection is added into a sliding window, relative pose transformation of the key frames in the sliding window is taken as pose matching measurement data, a matching method combining ORB characteristics and an original point selection strategy is adopted based on a word bag method, after a loop matching sub is selected, pose constraint of the matching sub on a current key frame is calculated and added into a global pose graph, and the pose of a camera is estimated along with optimization.
As shown in fig. 2, in the optimization of the sliding window, as new key frames and points are added, the size of the sliding window will increase continuously, and eventually, the flexible and efficient function thereof is lost. In order to maintain a fixed-size window, the invention uses an marginalization strategy to cull out old frames and old points. Marginalizing a point if the point is not already within the camera field of view; if the number of frames in the sliding window exceeds a set threshold value of 7 frames (the number of frames in the sliding window can be modified into 10 frames to improve the pose estimation precision), one of the frames is selected for marginalization; when a certain frame is marginalized, the map point taking the certain frame as the leading point is removed, and does not participate in calculation, so that the pose estimation accuracy is further improved;
meanwhile, closed loop detection is carried out by using pose graph optimization, a global pose is added in a sliding window to maintain the connection between key frames, the relative pose transformation of the key frames in the sliding window is taken as measurement data of pose pairing, a matching method combining ORB characteristics and an original point selection strategy is provided based on a bag-of-words method, and when loop matching is selectedThen, using 3 pairs of matching points to solve similarity transformation (similarity transformation), and further solving rotation matrix, translation vector and scale between two coordinate systems, i.e. Sim (3) transformation, so that matching sub-similarity transformation for current key frame is ScrThen S iscrBy minimizing a cost function ElTo obtain El=ω(ScrΠ-1(p,dp)-Π-1(q,dq))
Wherein p and q represent matched pixel points in the matched sub-frame and the key frame, dp、dqRepresenting the inverse depth corresponding to the pixel points p and q, and omega is a balance weight parameter. And mixing ScrAnd adding the pose estimation result into a global pose graph, and obtaining accurate camera pose estimation along with optimization.
As another embodiment, the scheme of the invention can be transplanted to an AGV (automated Guided vehicle) with a microphone mother wheel as a research platform, namely a PIBOT Hades robot main body, and a Kinect camera sensor is mounted. Field experiments were performed in an indoor corridor environment.
In order that those skilled in the art will better understand the disclosure, the invention will be described in further detail with reference to the accompanying drawings and specific embodiments. It is to be understood that the described embodiments are merely exemplary of the invention, and not restrictive of the full scope of the invention. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
Claims (10)
1. A mobile pose estimation method based on a closed-loop direct sparse visual odometer is characterized by comprising the following steps:
s1, acquiring a robot peripheral image sequence;
step S2, initializing based on the first two frames of images, and constructing a reference frame of a subsequent image sequence;
s3, acquiring relative pose change between frames and judging whether the pose change is a key frame;
step S4, adding a rear-end sliding window into the new key frame, and jointly constructing a total luminosity error function by the positions and postures of all the key frames in the window and the key points extracted from the key frames, and optimizing the initial value of the relative position and posture in front-end estimation;
and step S5, marginalizing the old key frames, constructing a global pose graph based on descriptors of all the key frames for closed-loop detection, and estimating the pose of the robot.
2. The method for estimating the motion pose based on the closed-loop direct sparse visual odometer according to claim 1, wherein the method comprises the following steps: step S2 is further to initialize the input of the camera sensor at the front end of the odometer; the initialization needs two frames of images, the first frame is used as a reference frame, namely a Gaussian pyramid is constructed for the images according to a world coordinate system of a camera coordinate system where the first frame is located, gradient histograms of all pixel points in the 0 th layer of the image pyramid are counted, different gradient points are extracted from the images layer by taking the number of bits as a gradient threshold, a Nearest neighbor (Fast Library for Approximate neighbor Neighbors) index is constructed for each gradient point, s points closest to the s gradient points are found in the same layer to construct a neighborhood relation, a point with the minimum distance to the current point is found in the Nearest neighbor index of the next layer and is set as an associated point, and the current layer and the next layer are associated, so that the spread of the optical flow pyramid and the inverse depth value is facilitated.
3. The method for estimating the motion pose based on the closed-loop direct sparse visual odometer according to claim 2, characterized in that: in step S2, let the second frame be IvThe first frame is Iu,IvBased on IuAnd constructing an interframe luminosity error function by using the selected key point with the maximum gradient:
Eu,v,w=ωp(Iv[p`]-avuIu[p]-bvu)
wherein ω ispTo compensate for the weighting parameter, p is IuPoint of medium gradient, p' denotes p in the current frame IvBack-projected coordinates of (1)vuCorrecting the parameters for the exposure time, bvuAs an imageA luminance floor correction parameter;
iteratively minimizing the photometric error by a gauss-newton method; if the error is optimized and not converged, judging that the initialization fails, resetting the first frame and repeating the process; otherwise, the initialization is successful, the first frame is set as a key frame, the gradient points extracted from the first frame are activated, and the gradient points are added into the optimization of the sliding window; the second frame enters sliding window optimization after initialization.
4. The method for estimating the motion pose based on the closed-loop direct sparse visual odometer according to claim 3, wherein the method comprises the following steps: the gradient point activation is divided into an activation point and an immature point, wherein the immature point is a point extracted from an image or not converged in depth; and the activated points are continuously filtered among frames through a depth filter, so that the depth of the points is continuously converged, and finally the obtained points are activated and added into a sliding window.
5. The method for estimating the motion pose based on the closed-loop direct sparse visual odometer according to claim 4, wherein the method comprises the following steps: step S3 is further to perform pose solution based on the nearest key frame for each input frame of new image, that is, project the gradient points of the nearest key frame into the current frame to construct a photometric error function:
Eu,v,w=ωp(Iv[p`]-avuIu[p]-bvu);
wherein ω ispTo compensate for the weighting parameter, p is IuPoint of medium gradient, p' denotes p in the current frame IvBack-projected coordinates of (1)vuCorrecting the parameters for the exposure time, bvuCorrecting parameters for the image brightness basis; and iteratively minimizing the luminosity error through a Gauss-Newton method, and if the error is optimized and converged, taking the new frame as a key frame and adding the key frame into a sliding window for optimization.
6. The method for estimating the motion pose based on the closed-loop direct sparse visual odometer according to claim 5, wherein the method comprises the following steps: in step S4, each point is projected into all key frames in the inter-frame pose solution by using a sparse direct method or a direct optical flow matching method, a residual error in each frame is calculated, a sliding window composed of a plurality of system key frames is used at the back end, all key frame poses and activation points in the sliding window are all combined to optimize a total luminosity error function, and when a frame is judged to be in accordance with a condition by the front end process and inserted into the back end, a minimum luminosity error to be optimized is defined as follows:
let the image frame set S ═ T1,T2…Tj,p1,p2…pkIn which T isjRepresenting the pose, p, of the keyframe jkRepresenting coordinates of pixel points in a keyframe, minimizing photometric errorsThe method comprises the following steps:
wherein, tu、tvAs an image Iu、IvA corresponding exposure time;is a collection of pixel points, pwIs a pixel point in the set, p is IvMiddle pixel point, avIs IvExposure time correction parameter of auIs IuExposure time correction parameter of (b)vIs IvLuminance floor correction parameter, buIs IuA luminance floor correction parameter; omegapTo compensate for the weighting parameter; p' is a depth parameter d containing the inversepIn the image I at the pixel coordinate pvThe reprojection coordinates of (c).
7. The method for estimating the motion pose based on the closed-loop direct sparse visual odometer according to claim 6, wherein the method comprises the following steps: the reprojection coordinate p' is formulated as follows:
p`=Π(RΠ-1(p,dp)+t)
r, T is a rotation matrix and a translation vector, and is transformed by a transformation matrix Tu、TvCalculated result, pi and pi-1Representing the reprojection and inverse reprojection transforms, respectively.
8. The method for estimating the motion pose based on the closed-loop direct sparse visual odometer according to claim 7, wherein the method comprises the following steps: in step S5, an old frame and an old point are removed by using a marginalization strategy, and if a point is not in the camera view, the point is marginalized; if the number of frames in the sliding window exceeds a set threshold value, selecting one frame for marginalization; when a frame is marginalized, the map points that dominate it will be removed.
9. The method for estimating the motion pose based on the closed-loop direct sparse visual odometer according to claim 8, wherein the method comprises the following steps: step S5, using pose graph optimization to perform closed loop detection, adding global pose in a sliding window to maintain the connection between key frames, taking the relative pose transformation of the key frames in the sliding window as the measurement data of pose matching, based on a matching method combining ORB characteristics and an original point selection strategy by a bag-of-words method, after selecting a loop matching sub, calculating and calculating the similar transformation of the matching sub to the current key frame, adding the similar transformation to the global pose graph, and estimating the pose of the camera along with optimization.
10. The method for estimating the motion pose based on the closed-loop direct sparse visual odometer according to claim 9, wherein the method comprises the following steps:
using h pair matching points to solve similarity transformation (similarity transformation), and further solving rotation matrix, translation vector and scale between two coordinate systems, i.e. sim (h) transformation, so that matching sub-similarity transformation of current key frame is ScrThen S iscrBy minimizing a cost function ElObtaining:
El=ω(ScrΠ-1(p,dp)-Π-1(q,dq))
wherein p and q represent matched pixel points in the matched sub-and key-frames, dp、dqRepresenting the inverse depth corresponding to the pixel points p and q, and omega is a balance weight parameter.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110245806.7A CN113108771B (en) | 2021-03-05 | 2021-03-05 | Movement pose estimation method based on closed-loop direct sparse visual odometer |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110245806.7A CN113108771B (en) | 2021-03-05 | 2021-03-05 | Movement pose estimation method based on closed-loop direct sparse visual odometer |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113108771A true CN113108771A (en) | 2021-07-13 |
CN113108771B CN113108771B (en) | 2022-08-16 |
Family
ID=76710961
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110245806.7A Active CN113108771B (en) | 2021-03-05 | 2021-03-05 | Movement pose estimation method based on closed-loop direct sparse visual odometer |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113108771B (en) |
Cited By (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113673462A (en) * | 2021-08-27 | 2021-11-19 | 西安电子科技大学广州研究院 | Logistics AGV positioning method based on lane line |
CN113790728A (en) * | 2021-09-29 | 2021-12-14 | 佛山市南海区广工大数控装备协同创新研究院 | Loosely-coupled multi-sensor fusion positioning algorithm based on visual odometer |
CN114581517A (en) * | 2022-02-10 | 2022-06-03 | 北京工业大学 | Improved VINS method for complex illumination environment |
CN114663496A (en) * | 2022-03-23 | 2022-06-24 | 北京科技大学 | Monocular vision odometer method based on Kalman pose estimation network |
CN115170992A (en) * | 2022-09-07 | 2022-10-11 | 山东水发达丰再生资源有限公司 | Image identification method and system for scattered blanking of scrap steel yard |
CN115451996A (en) * | 2022-08-30 | 2022-12-09 | 华南理工大学 | Homography vision mileometer method for indoor environment |
CN115937011A (en) * | 2022-09-08 | 2023-04-07 | 安徽工程大学 | Keyframe pose optimization vision SLAM method based on time lag feature regression, storage medium and equipment |
CN117826141A (en) * | 2023-12-29 | 2024-04-05 | 广东工业大学 | Collaborative positioning method for distributed unmanned aerial vehicle group in complex environment |
Citations (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106846417A (en) * | 2017-02-06 | 2017-06-13 | 东华大学 | The monocular infrared video three-dimensional rebuilding method of view-based access control model odometer |
CN107610175A (en) * | 2017-08-04 | 2018-01-19 | 华南理工大学 | The monocular vision SLAM algorithms optimized based on semi-direct method and sliding window |
CN108986037A (en) * | 2018-05-25 | 2018-12-11 | 重庆大学 | Monocular vision odometer localization method and positioning system based on semi-direct method |
CN109544636A (en) * | 2018-10-10 | 2019-03-29 | 广州大学 | A kind of quick monocular vision odometer navigation locating method of fusion feature point method and direct method |
CN109631896A (en) * | 2018-07-23 | 2019-04-16 | 同济大学 | A kind of parking lot autonomous parking localization method based on vehicle vision and motion information |
CN110866496A (en) * | 2019-11-14 | 2020-03-06 | 合肥工业大学 | Robot positioning and mapping method and device based on depth image |
CN111462207A (en) * | 2020-03-30 | 2020-07-28 | 重庆邮电大学 | RGB-D simultaneous positioning and map creation method integrating direct method and feature method |
CN111693047A (en) * | 2020-05-08 | 2020-09-22 | 中国航空工业集团公司西安航空计算技术研究所 | Visual navigation method for micro unmanned aerial vehicle in high-dynamic scene |
CN111780754A (en) * | 2020-06-23 | 2020-10-16 | 南京航空航天大学 | Visual inertial odometer pose estimation method based on sparse direct method |
WO2020244717A1 (en) * | 2019-06-05 | 2020-12-10 | Conti Temic Microelectronic Gmbh | Detection, 3d reconstruction and tracking of multiple rigid objects moving in relation to one another |
CN112419497A (en) * | 2020-11-13 | 2021-02-26 | 天津大学 | Monocular vision-based SLAM method combining feature method and direct method |
-
2021
- 2021-03-05 CN CN202110245806.7A patent/CN113108771B/en active Active
Patent Citations (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106846417A (en) * | 2017-02-06 | 2017-06-13 | 东华大学 | The monocular infrared video three-dimensional rebuilding method of view-based access control model odometer |
CN107610175A (en) * | 2017-08-04 | 2018-01-19 | 华南理工大学 | The monocular vision SLAM algorithms optimized based on semi-direct method and sliding window |
CN108986037A (en) * | 2018-05-25 | 2018-12-11 | 重庆大学 | Monocular vision odometer localization method and positioning system based on semi-direct method |
CN109631896A (en) * | 2018-07-23 | 2019-04-16 | 同济大学 | A kind of parking lot autonomous parking localization method based on vehicle vision and motion information |
CN109544636A (en) * | 2018-10-10 | 2019-03-29 | 广州大学 | A kind of quick monocular vision odometer navigation locating method of fusion feature point method and direct method |
WO2020244717A1 (en) * | 2019-06-05 | 2020-12-10 | Conti Temic Microelectronic Gmbh | Detection, 3d reconstruction and tracking of multiple rigid objects moving in relation to one another |
CN110866496A (en) * | 2019-11-14 | 2020-03-06 | 合肥工业大学 | Robot positioning and mapping method and device based on depth image |
CN111462207A (en) * | 2020-03-30 | 2020-07-28 | 重庆邮电大学 | RGB-D simultaneous positioning and map creation method integrating direct method and feature method |
CN111693047A (en) * | 2020-05-08 | 2020-09-22 | 中国航空工业集团公司西安航空计算技术研究所 | Visual navigation method for micro unmanned aerial vehicle in high-dynamic scene |
CN111780754A (en) * | 2020-06-23 | 2020-10-16 | 南京航空航天大学 | Visual inertial odometer pose estimation method based on sparse direct method |
CN112419497A (en) * | 2020-11-13 | 2021-02-26 | 天津大学 | Monocular vision-based SLAM method combining feature method and direct method |
Non-Patent Citations (3)
Title |
---|
LUKAS VON STUMBERG等: "Direct Sparse Visual-Inertial Odometry Using Dynamic Marginalization", 《2018 IEEE INTERNATIONAL CONFERENCE ON ROBOTICS AND AUTOMATION(ICRA)》 * |
李奎霖等: "基于LDSO的机器人视觉定位与稠密建图的应用", 《微电子学与计算机》 * |
潘林豪等: "基于直接法的视觉同时定位与地图构建技术综述", 《计算机应用研究》 * |
Cited By (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113673462A (en) * | 2021-08-27 | 2021-11-19 | 西安电子科技大学广州研究院 | Logistics AGV positioning method based on lane line |
CN113673462B (en) * | 2021-08-27 | 2023-12-19 | 西安电子科技大学广州研究院 | Logistics AGV positioning method based on lane lines |
CN113790728A (en) * | 2021-09-29 | 2021-12-14 | 佛山市南海区广工大数控装备协同创新研究院 | Loosely-coupled multi-sensor fusion positioning algorithm based on visual odometer |
CN114581517A (en) * | 2022-02-10 | 2022-06-03 | 北京工业大学 | Improved VINS method for complex illumination environment |
CN114663496A (en) * | 2022-03-23 | 2022-06-24 | 北京科技大学 | Monocular vision odometer method based on Kalman pose estimation network |
CN114663496B (en) * | 2022-03-23 | 2022-10-18 | 北京科技大学 | Monocular vision odometer method based on Kalman pose estimation network |
CN115451996A (en) * | 2022-08-30 | 2022-12-09 | 华南理工大学 | Homography vision mileometer method for indoor environment |
CN115451996B (en) * | 2022-08-30 | 2024-03-29 | 华南理工大学 | Homography visual odometer method facing indoor environment |
CN115170992A (en) * | 2022-09-07 | 2022-10-11 | 山东水发达丰再生资源有限公司 | Image identification method and system for scattered blanking of scrap steel yard |
CN115937011A (en) * | 2022-09-08 | 2023-04-07 | 安徽工程大学 | Keyframe pose optimization vision SLAM method based on time lag feature regression, storage medium and equipment |
CN115937011B (en) * | 2022-09-08 | 2023-08-04 | 安徽工程大学 | Key frame pose optimization visual SLAM method, storage medium and equipment based on time lag feature regression |
CN117826141A (en) * | 2023-12-29 | 2024-04-05 | 广东工业大学 | Collaborative positioning method for distributed unmanned aerial vehicle group in complex environment |
Also Published As
Publication number | Publication date |
---|---|
CN113108771B (en) | 2022-08-16 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN113108771B (en) | Movement pose estimation method based on closed-loop direct sparse visual odometer | |
CN108537848B (en) | Two-stage pose optimization estimation method for indoor scene reconstruction | |
CN109166149B (en) | Positioning and three-dimensional line frame structure reconstruction method and system integrating binocular camera and IMU | |
CN108986037B (en) | Monocular vision odometer positioning method and positioning system based on semi-direct method | |
CN111210463B (en) | Virtual wide-view visual odometer method and system based on feature point auxiliary matching | |
CN110108258B (en) | Monocular vision odometer positioning method | |
CN104732518B (en) | A kind of PTAM improved methods based on intelligent robot terrain surface specifications | |
CN110570453B (en) | Binocular vision-based visual odometer method based on closed-loop tracking characteristics | |
CN113256698B (en) | Monocular 3D reconstruction method with depth prediction | |
CN116205947A (en) | Binocular-inertial fusion pose estimation method based on camera motion state, electronic equipment and storage medium | |
CN111932674A (en) | Optimization method of line laser vision inertial system | |
CN112418288A (en) | GMS and motion detection-based dynamic vision SLAM method | |
CN114001733A (en) | Map-based consistency efficient visual inertial positioning algorithm | |
CN111882602A (en) | Visual odometer implementation method based on ORB feature points and GMS matching filter | |
CN114494150A (en) | Design method of monocular vision odometer based on semi-direct method | |
CN114964276A (en) | Dynamic vision SLAM method fusing inertial navigation | |
CN117367427A (en) | Multi-mode slam method applicable to vision-assisted laser fusion IMU in indoor environment | |
CN115471748A (en) | Monocular vision SLAM method oriented to dynamic environment | |
CN113345032B (en) | Initialization map building method and system based on wide-angle camera large distortion map | |
Zhu et al. | PairCon-SLAM: Distributed, online, and real-time RGBD-SLAM in large scenarios | |
CN113362377B (en) | VO weighted optimization method based on monocular camera | |
CN115147344A (en) | Three-dimensional detection and tracking method for parts in augmented reality assisted automobile maintenance | |
CN112767481A (en) | High-precision positioning and mapping method based on visual edge features | |
CN112419411A (en) | Method for realizing visual odometer based on convolutional neural network and optical flow characteristics | |
CN116804553A (en) | Odometer system and method based on event camera/IMU/natural road sign |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |