CN113108771B - 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
- CN113108771B CN113108771B CN202110245806.7A CN202110245806A CN113108771B CN 113108771 B CN113108771 B CN 113108771B CN 202110245806 A CN202110245806 A CN 202110245806A CN 113108771 B CN113108771 B CN 113108771B
- 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.)
- Active
Links
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)
- Manipulator (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (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 angular points, and meanwhile, the accumulated errors such as 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 I v The first frame is I u ，I v Based on I u The selected key point with the maximum gradient is used for constructing interframe luminosityError function:
E u，v，w ＝ω p (I v [p｀]-a vu I u [p]-b vu )
wherein ω is p To compensate for the weighting parameter, p is I u Point of medium gradient, p' denotes p is in the current frame I v Back-projected coordinates of (1) vu Correcting the parameters for the exposure time, b vu Correcting 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:
E u，v，w ＝ω p (I v [p`]-a vu I u [p]-b vu )；
wherein ω is p To compensate for the weighting parameter, p is I u Point of medium gradient, p' denotes p in the current frame I v Back-projected coordinates of (1) vu Correcting the parameters for the exposure time, b vu Correcting parameters for the image brightness base; 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 ═ T 1 ，T 2 …T j ，p 1 ，p 2 …p k In which T is j Representing the pose, p, of the keyframe j k Representing coordinates of pixel points in a keyframe, minimizing photometric errors
The method comprises the following steps:
wherein, t u 、t v Is an image I u 、I v A corresponding exposure time;
is a collection of pixel points, p w Is a pixel point in the set, p is I v Middle pixel point, a v Is I v Exposure time correction parameter of a u Is I u Exposure time correction parameter of (b) v Is I v Luminance floor correction parameter, b u Is I u A luminance floor correction parameter; omega p To compensate for the weighting parameter; p' is a constant value containing an inverse depth parameter d p In the image I at the pixel coordinate p v The reprojection coordinates of (c).
Preferably, the formula of the reprojection coordinate p' is as follows:
p`＝Π(RΠ -1 (p，d p )+t)
r, T is a rotation matrix and a translation vector, and is transformed by a transformation matrix T u 、T v Calculated result, pi and pi -1 Respectively representing the reprojection and the inverseAnd (5) carrying out reprojection transformation.
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 performs closed-loop detection using pose graph optimization, adds global pose in a sliding window to maintain the connection between key frames, takes the relative pose transformation of key frames in the sliding window as the measurement data of pose pairs, based on a matching method that combines the ORB features with the original point selection strategy using the bag-of-words approach, calculates the similar transformation of matching pairs to the current key frame after selecting the loop matching pair, and adds it to the global pose graph, with the optimization to estimate the pose of the camera.
Preferably, h is used to solve similarity transformation (similarity transformation) for the matching points, and further solve rotation matrix, translation vector and scale between two coordinate systems, i.e. sim (h) transformation, so that matching sub-similarity transformation for the current key frame is S cr Then S is cr By minimizing a cost function E l Obtaining:
E l ＝ω(S cr Π -1 (p，d p )-Π -1 (q，d q ))
wherein p and q represent matched pixel points in the matched sub-and key-frames, d p 、d q Representing 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 points 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 I v The first frame is I u ，I v Based on I u And constructing an interframe luminosity error function by using the selected key points with larger gradients:
E u，v，w ＝ω p (I v [p`]-a vu I u [p]-b vu )
wherein ω is p To compensate for the weighting parameter, p is I u The point with larger gradient in the middle, p' represents that p is in the current frame I v Back-projected coordinates of (1) vu Correcting the parameters for the exposure time, b vu Correcting 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.
Step S3, resolving the relative pose change between frames based on the direct method of the minimum luminosity error and judging whether the relative pose change is a key frame;
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:
E u，v，w ＝ω p (I v [p`]-a vu I u [p]-b vu )
wherein ω is p To compensate for the weighting parameter, p is I u The point with larger gradient in the middle, p' represents that p is in the current frame I v Back-projected coordinates of (1) vu Correcting the parameters for the exposure time, b vu Correcting 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 ═ T 1 ，T 2 …T j ，p 1 ，p 2 …p k In which T is j Representing the pose, p, of the keyframe j k Representing coordinates of pixel points in keyframes, minimizing photometric errors
The method comprises the following steps:
wherein, t u 、t v As an image I u 、I v A corresponding exposure time;
is a collection of pixel points, p w Is a pixel point in the set, p is I v Middle pixel point, a v Is I v Exposure time correction parameter of a u Is I u Exposure time correction parameter of (b) v Is shown as I v Luminance floor correction parameter, b u Is I u Luminance floor correction parameter, ω p To compensate for the weighting parameter; p' is a depth parameter d containing the inverse p In the image I at the pixel coordinate p v The formula p' is as follows:
p`＝Π(RΠ -1 (p，d p )+t)
r, T is a rotation matrix and a translation vector, and is transformed by a transformation matrix T u 、T v Calculated result, pi and pi -1 Representing 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, performing marginalization on the old key frames, and constructing a global pose graph for closed-loop detection based on the descriptors of all the key frames. 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 marginalizing; 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 connection between key frames, 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 with an original point selection strategy is provided based on a bag-of-words method, when a loop matching sub-tree is selected, 3 pairs of matching points are used for solving similarity transformation (similarity transformation), and then a rotation matrix, a translation vector and a scale between two coordinate systems are solved, namely Sim (3) transformation is carried out, so that the similarity transformation of the matching sub-tree to the current key frame is S cr Then S is cr By minimizing a cost function E l To obtain E l ＝ω(S cr Π -1 (p，d p )-Π -1 (q，d q ))
Wherein p and q represent matched pixel points in the matched sub-frame and the key frame, d p 、d q Representing the inverse depth corresponding to the pixel points p and q, and omega is a balance weight parameter. And mixing S cr And 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 (9)
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: let the second frame be I v The first frame is I u ，I v Based on I u And constructing an interframe luminosity error function by using the selected key point with the maximum gradient:
E u，v，w ＝ω p (I v [p`]-a vu I u [p]-b vu )
wherein ω is p To compensate for the weighting parameter, p is I u Point of medium gradient, p' denotes p is in the current frame I v Back-projected coordinates of (1) vu Correction of parameters for exposure time, b vu Correcting 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; after the second frame is initialized, entering sliding window optimization;
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, performing marginalization on 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, 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, different gradient points are extracted from the images layer by taking the number of bits as a gradient threshold, a nearest neighbor index is constructed for each gradient point, s points closest to the points in the same layer are found to construct a neighborhood relationship, a point with the minimum distance to a 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 an optical flow pyramid and an 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: 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.
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: step S3 is further to perform pose calculation based on the nearest key frame for each input frame of new image, and project the gradient points of the nearest key frame into the current frame to construct a photometric error function:
E u，v，w ＝ω p (I v [p`]-a vu I u [p]-b vu )；
wherein ω is p To compensate for the weighting parameter, p is I u Point of medium gradient, p' denotes p is in the current frame I v Back-projected coordinates of (1) vu Correcting the parameters for the exposure time, b vu Correcting 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.
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: 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 ═ T 1 ，T 2 …T j ，p 1 ，p 2 …p k In which T is j Representing the pose, p, of the keyframe j k Representing coordinates of pixel points in a keyframe, minimizing photometric errors
The method comprises the following steps:
wherein, t u 、t v As an image I u 、I v A corresponding exposure time;
is a collection of pixel points, p w Is a pixel point in the set, p is I v Middle pixel point, a v Is I v Exposure time correction parameter of a u Is I u Exposure time correction parameter of (b) v Is I v Luminance floor correction parameter, b u Is I u A luminance floor correction parameter; omega p To compensate for the weighting parameter; p' is a depth parameter d containing the inverse p In the image I at the pixel coordinate p v The reprojection coordinates of (c).
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: the formula of the reprojection coordinate p' is as follows:
p`＝∏(R∏ -1 (p，d p )+t)
wherein R, T is rotation matrix and translation vector, and is transformed by transformation matrix T u 、T v Calculate pi and pi -1 Representing the reprojection and inverse reprojection transforms, respectively.
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: 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.
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: 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 with an original point selection strategy by a bag-of-words method, after selecting a loop matching sub, 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.
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:
using h to solve the similarity transformation of the matching points, further solving the rotation matrix, the translation vector and the scale between the two coordinate systems, sim (h) transformation, and leading the similarity transformation of the matching sub-pair to the current key frame to be S cr Then S is cr By minimizing a cost function E l Obtaining:
E l ＝ω(S cr ∏ -1 (p，d p )-∏ -1 (q，d q ))
wherein p and q represent matched pixel points in the matched sub-and key-frames, d p 、d q Representing 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 CN113108771A (en) | 2021-07-13 |
| CN113108771B true 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) |
Families Citing this family (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| 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 |
| CN114663496B (en) * | 2022-03-23 | 2022-10-18 | 北京科技大学 | Monocular vision odometer method based on Kalman pose estimation network |
| CN115451996B (en) * | 2022-08-30 | 2024-03-29 | 华南理工大学 | Homography visual odometer method facing indoor environment |
| CN115170992B (en) * | 2022-09-07 | 2022-12-06 | 山东水发达丰再生资源有限公司 | Image identification method and system for scattered blanking of scrap steel yard |
| 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 |
Family Cites Families (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 |
| CN108986037B (en) * | 2018-05-25 | 2020-06-16 | 重庆大学 | Monocular vision odometer positioning method and positioning system based on semi-direct method |
| CN109631896B (en) * | 2018-07-23 | 2020-07-28 | 同济大学 | Parking lot autonomous parking positioning method based on vehicle vision and motion information |
| CN109544636B (en) * | 2018-10-10 | 2022-03-15 | 广州大学 | Rapid monocular vision odometer navigation positioning method integrating feature point method and direct method |
| DE102019208216A1 (en) * | 2019-06-05 | 2020-12-10 | Conti Temic Microelectronic Gmbh | Detection, 3D reconstruction and tracking of several rigid objects moving relative to one another |
| CN110866496B (en) * | 2019-11-14 | 2023-04-07 | 合肥工业大学 | 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 |
| CN111693047B (en) * | 2020-05-08 | 2022-07-05 | 中国航空工业集团公司西安航空计算技术研究所 | Visual navigation method for micro unmanned aerial vehicle in high-dynamic scene |
| CN111780754B (en) * | 2020-06-23 | 2022-05-20 | 南京航空航天大学 | 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 |
-
2021
- 2021-03-05 CN CN202110245806.7A patent/CN113108771B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN113108771A (en) | 2021-07-13 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113108771B (en) | Movement pose estimation method based on closed-loop direct sparse visual odometer | |
| CN109166149B (en) | Positioning and three-dimensional line frame structure reconstruction method and system integrating binocular camera and IMU | |
| CN108537848B (en) | Two-stage pose optimization estimation method for indoor scene reconstruction | |
| CN109029433B (en) | Method for calibrating external parameters and time sequence based on vision and inertial navigation fusion SLAM on mobile platform | |
| CN111210463B (en) | Virtual wide-view visual odometer method and system based on feature point auxiliary matching | |
| Kneip et al. | Robust real-time visual odometry with a single camera and an IMU | |
| CN110570453B (en) | Binocular vision-based visual odometer method based on closed-loop tracking characteristics | |
| CN110533720B (en) | Semantic SLAM system and method based on joint constraint | |
| CN111932674A (en) | Optimization method of line laser vision inertial system | |
| CN111882602B (en) | Visual odometer implementation method based on ORB feature points and GMS matching filter | |
| CN112258409A (en) | Monocular camera absolute scale recovery method and device for unmanned driving | |
| CN112418288A (en) | GMS and motion detection-based dynamic vision SLAM method | |
| CN110390685A (en) | Feature point tracking method based on event camera | |
| CN114001733A (en) | Map-based consistency efficient visual inertial positioning algorithm | |
| CN115936029A (en) | SLAM positioning method and device based on two-dimensional code | |
| CN114494150A (en) | Design method of monocular vision odometer based on semi-direct method | |
| CN112541423A (en) | Synchronous positioning and map construction method and system | |
| CN114964276A (en) | Dynamic vision SLAM method fusing inertial navigation | |
| CN115147344A (en) | Three-dimensional detection and tracking method for parts in augmented reality assisted automobile maintenance | |
| CN113362377B (en) | VO weighted optimization method based on monocular camera | |
| Zhu et al. | PairCon-SLAM: Distributed, online, and real-time RGBD-SLAM in large scenarios | |
| CN113345032A (en) | Wide-angle camera large-distortion image based initial image construction method and system | |
| CN117367427A (en) | Multi-mode slam method applicable to vision-assisted laser fusion IMU in indoor environment | |
| Lebegue et al. | Extraction and interpretation of semantically significant line segments for a mobile robot | |
| CN112419411A (en) | Method for realizing visual odometer based on convolutional neural network and optical flow characteristics |
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 |