A monocular-based indoor office scene visual positioning method and system

Abstract

The application relates to a monocular-based indoor office scene visual positioning method and system, and belongs to the technical field of visual positioning, and solves the scale uncertainty and scale drift problems of the existing monocular SLAM. The method comprises the following steps: target detection and tracking are performed on each frame of real-time collected images to obtain feature point information and target detection information of each frame of images; when a two-dimensional code is detected, the camera pose corresponding to each frame of images in the window is estimated through an optimization method of epipolar geometry and re-projection error, and is converted into a coordinate system with the two-dimensional code center as the origin; camera pose optimization processing is started by acquiring a new frame of images, including predicting the camera pose corresponding to the new image, establishing error functions for each frame of images in the window and the new image, calculating the solution with the minimum error square sum, obtaining the optimized camera pose and feature point inverse depth, sliding the window, continuously acquiring a new frame of images, and performing camera pose optimization processing. Precise positioning of an indoor office scene is realized.

Description

Technical Field

[0001] This invention relates to the field of visual positioning technology, and in particular to a visual positioning method and system for indoor office scenes based on monocular vision. Background Technology

[0002] SLAM (simultaneous localization and mapping) is a technique that uses vision to perform simultaneous localization and mapping. Currently, monocular SLAM schemes mainly include ORBSLAM, PTAM, and DSO. However, regardless of the monocular SLAM scheme, there are two problems: one is the scale uncertainty problem, which means that the objective and actual scale cannot be obtained; the other is the scale drift problem, which means that as the cumulative error of state estimation continues, the originally defined relative scale will drift.

[0003] To address the two issues mentioned above, most methods employ multi-sensor fusion, such as binocular SLAM using two monocular cameras. Binocular SLAM can statically calculate depth while avoiding scale uncertainty and scale drift. However, calculating pixel distances from binocular images is computationally intensive and requires precise calibration of the transformation relationship between the two cameras.

[0004] Besides binocular SLAM, using a combination of monocular and IMU is also a popular method. This method also requires precise calibration of the transformation relationship between the IMU and the camera, as well as calibration of the IMU's intrinsic parameters. Multi-sensor fusion reduces the difficulty of the SLAM problem, but it also increases the basic workload and cost. Summary of the Invention

[0005] Based on the above analysis, the embodiments of the present invention aim to provide an indoor scene visual localization method based on the combination of monocular and deep learning, in order to solve the problems of scale uncertainty and scale drift in existing monocular SLAM.

[0006] On one hand, embodiments of the present invention provide a visual positioning method for indoor office scenes based on monocular vision, comprising the following steps:

[0007] Load target information, perform target detection on each frame of image acquired in real time, and obtain target detection results; targets include: 1 QR code, at least 1 fire hydrant, and door;

[0008] Track each frame of the image to obtain the feature point information and target detection information of each frame;

[0009] When there is a QR code target in the target detection information, the image is placed into the window according to the preset window size. Through the optimization method of epipolar geometry and reprojection error, the camera pose corresponding to each frame of the image in the window is estimated, and the camera pose is transformed into the origin coordinate system with the center of the QR code as the origin.

[0010] To acquire a new image frame, begin camera pose optimization processing, which includes: using a uniform velocity model to predict the camera pose corresponding to the new image; for each image frame and the new image within the window, establishing error functions based on the prior camera pose, the camera pose, the coordinates of the QR code corner points, the inverse depth of the feature points, the relative poses of adjacent frames, and the height of the gate; simultaneously solving the system of error functions to calculate the solution that minimizes the sum of squared errors; obtaining the optimized camera pose and inverse depth of the feature points corresponding to each image frame and the new image within the window; sliding the window to acquire another new image and repeating the camera pose optimization processing.

[0011] Based on the further improvement of the above method, each frame of the image is tracked to obtain the feature point information and target detection information of each frame, including:

[0012] The feature points of each frame of the image are obtained, and the feature points of each frame of the image are tracked using the optical flow method to obtain the feature point information, including: feature point pixel coordinates and feature point number. Among them, the feature points that are successfully tracked have the same feature point number.

[0013] Based on the successfully tracked feature points, obtain the average movement vector of the feature points in the target box in the previous frame and the current frame, and based on the average movement vector, obtain the target box in the current frame in the previous frame, which is used as the first target box.

[0014] When the intersection-union ratio (IUU) of the first target bounding box and the target bounding box in the previous frame is greater than the threshold, the target bounding box in the current frame is successfully associated with the target bounding box in the previous frame. The same target bounding box number is set and added to the target detection result to obtain the target detection information.

[0015] Based on the further improvement of the above method, when there is a QR code target in the target detection information, it also includes: taking the center of the QR code as the origin, obtaining the three-dimensional absolute coordinates of the corner points of the QR code according to the size of the QR code in the loaded target information, and using the P3P algorithm to calculate the camera prior pose corresponding to the image where the QR code is located.

[0016] Based on the preset window size, the image is placed into the window. This also includes: obtaining the three-dimensional coordinates of the feature points in the first window image in the camera coordinate system through triangulation, and obtaining the normalized planar coordinates and inverse depth of the feature points based on their three-dimensional coordinates.

[0017] Based on the above method, a further improvement is made to transform the poses of each camera to the origin coordinate system with the center of the QR code as the origin, including:

[0018] The first relative pose is obtained based on the camera prior poses corresponding to the first and last images with detected QR codes in the window.

[0019] The second relative pose is obtained based on the estimated camera poses corresponding to the first and last images of the detected QR codes within the window;

[0020] The ratio of the translation in the first relative pose to the translation in the second relative pose is used as a scale factor. The translation in each camera pose is multiplied by the scale factor to obtain the camera pose with absolute scale.

[0021] Based on the prior camera pose and the pose with absolute scale corresponding to the first image in the window that detects the QR code, the coordinate transformation matrix is ​​obtained. Then, it is multiplied by the camera pose with absolute scale to obtain the pose of each camera in the origin coordinate system.

[0022] A further improvement to the above method is to establish an error function based on the prior camera pose and the current camera pose. This is done by establishing the prior camera pose corresponding to the image of the detected QR code and the corresponding current camera pose.

[0023] Further improvements to the above method involve establishing an error function based on the corner coordinates of the QR code. This is achieved by comparing the normalized plane coordinates of the three-dimensional absolute coordinate projection of each corner point with the observed normalized plane coordinates of each corner point in the detected QR code image, and then establishing a reprojection error function for each corner point.

[0024] Further improvements to the above method involve establishing an error function based on the inverse depth of feature points. This is achieved by taking a set of images with the same feature point number and projecting the normalized planar coordinates and inverse depth of the feature points in the camera coordinate system onto any other frame in the set. The normalized planar coordinates of the projected points are obtained by projecting them onto the image where the feature points were first observed. These normalized planar coordinates are then compared with the normalized planar coordinates of the feature points observed in another frame to establish the inverse projection error function.

[0025] Further improvements to the above method involve establishing an error function based on the relative pose of adjacent frames. This is done by calculating the relative observation pose of adjacent frames with the same target box number and whose target is a QR code or a fire hydrant, and then using the relative pose obtained from the camera pose of the adjacent frames to establish the observation error function.

[0026] Further improvements to the above method involve establishing an error function based on the door height. This is achieved by converting the coordinates of any corner point on either side of the detected door in the origin coordinate system to three-dimensional coordinates in the camera coordinate system, calculating the door height, and comparing it with the actual door height corresponding to the target in the loaded target information to establish a door height error function.

[0027] On the other hand, embodiments of the present invention provide a visual positioning system for indoor office scenes based on monocular vision, including:

[0028] The target detection module is used to load target information, perform target detection on each frame of image acquired in real time, and obtain target detection results; the targets include: 1 QR code, at least 1 fire hydrant, and a door;

[0029] The target tracking module is used to track the target detection results and obtain the feature point information and target detection information of each frame of image;

[0030] The visual initialization module is used to start placing image frames into the window according to the preset window size when there is a QR code target in the target detection information. Through the optimization method of epipolar geometry and reprojection error, the camera pose corresponding to each frame of the image in the window is estimated, and the camera poses are transformed into the origin coordinate system with the center of the QR code as the origin.

[0031] The pose optimization module is used to acquire a new image frame and start camera pose optimization processing, including: using a uniform velocity model to predict the camera pose corresponding to the new image; for each image frame and the new image within the window, establishing error functions based on the prior camera pose, camera pose, QR code corner coordinates, inverse depth of feature points, relative poses of adjacent frames, and the height of the gate; solving the solution with the minimum sum of squared errors by combining the error functions; obtaining the optimized camera pose and inverse depth of feature points corresponding to each image frame and the new image within the window; sliding the window to acquire another new image and repeating the camera pose optimization processing.

[0032] Compared with existing technologies, this invention achieves at least one of the following beneficial effects: It enables SLAM localization of indoor scenes using information from fire hydrants, doors, and QR codes without increasing hardware costs; by combining monocular vision with deep learning object detection, it solves the scale uncertainty problem inherent in monocular vision; simultaneously, by using the relative error constructed through object detection to constrain motion between two frames, it effectively avoids scale drift; and by adding absolute coordinates to QR codes, in large-scale scenes, the camera pose can be corrected in a timely manner when a QR code is seen, avoiding the problem of infinite amplification of accumulated errors. This invention not only avoids the scale uncertainty and scale drift problems inherent in monocular SLAM but also improves localization accuracy.

[0033] In this invention, the above-described technical solutions can be combined with each other to achieve more preferred combinations. Other features and advantages of this invention will be set forth in the following description, and some advantages may become apparent from the description or be learned by practicing the invention. The objects and other advantages of this invention can be realized and obtained from what is particularly pointed out in the description and drawings. Attached Figure Description

[0034] The accompanying drawings are for illustrative purposes only and are not intended to limit the invention. Throughout the drawings, the same reference numerals denote the same parts.

[0035] Figure 1 This is a flowchart of the visual positioning method for indoor office scenes based on monocular vision in Embodiment 1 of the present invention. Detailed Implementation

[0036] Preferred embodiments of the present invention will now be described in detail with reference to the accompanying drawings, which form part of this application and are used together with the embodiments of the present invention to illustrate the principles of the present invention, but are not intended to limit the scope of the present invention.

[0037] Example 1

[0038] A specific embodiment of the present invention discloses a visual positioning method for indoor office scenes based on monocular vision, such as... Figure 1 As shown, it includes the following steps:

[0039] S11: Load target information, perform target detection on each frame of image acquired in real time, and obtain target detection results; targets include: 1 QR code, at least 1 fire hydrant and door.

[0040] It should be noted that this embodiment is based on an indoor office scenario, and the targets involved include: one QR code, at least one fire hydrant and a door. The unique QR code can be located anywhere indoors and is used to mark the origin and define the origin coordinate system.

[0041] The target information to be loaded includes: the size information of the QR code, the size information of the door and fire hydrant, such as the type of door, the height of each type of door, and the height and width information of the fire hydrant.

[0042] Target detection is performed on each frame of image captured in real time by the camera, which includes two parts: one part is to detect fire hydrants and doors in the image using a deep learning framework, such as the YOLO-V5 framework; the other part is to use the AprilTag algorithm to detect QR codes in the image, obtain the pixel coordinates of the QR code corners, and label the target type as QR code; finally, the target detection results include: target type and the pixel coordinates of the four corners of the target in the image.

[0043] This embodiment solves the scale uncertainty problem of monocular vision by combining monocular vision with deep learning object detection.

[0044] S12: Track each frame of the image to obtain the feature point information and target detection information of each frame.

[0045] It should be noted that the Shi-Tomasi algorithm is used to detect feature points in each frame of the image, based on the upper limit of the number of feature points to be detected. For example, the upper limit of the number of feature points to be detected is set to 150.

[0046] The optical flow method is used to track feature points in each frame of the image and determine the matching relationship between feature points in consecutive frames. The obtained feature point information includes: feature point pixel coordinates and feature point number. Among them, the successfully tracked feature points have the same feature point number.

[0047] Based on the successfully tracked feature points, target bounding boxes are then tracked, allowing the targets detected in adjacent frames to be associated, thus determining the correspondence between target bounding boxes in adjacent frames. Specifically, this includes:

[0048] ① Based on the successfully tracked feature points, obtain the average movement vector of the feature points in the target box in the previous frame and the current frame, and based on the average movement vector, obtain the target box in the current frame in the previous frame, which is used as the first target box;

[0049] Specifically, the average movement vector of the feature points is expressed by the following formula:

[0050]

[0051] Where N is the number of feature points successfully tracked. Let n be the pixel coordinates of the nth feature point in the current frame. The pixel coordinates of the nth feature point in the previous frame.

[0052] If a feature point in a target box in the current frame is in multiple target boxes in the current frame, the target box containing the most tracked feature points in the current frame is taken as the target box corresponding to the target box in the previous frame. The average movement vector is subtracted from the coordinates of its four corner points to obtain the target box in the previous frame, which is then taken as the first target box.

[0053] ② When the intersection-union ratio of the first target box and the target box in the previous frame is greater than the threshold, the target box in the current frame is successfully associated with the target box in the previous frame. The same target box number is set and added to the target detection result to obtain the target detection information.

[0054] Preferably, the crossover ratio threshold is set to 0.9.

[0055] Furthermore, based on the actual image acquisition frequency, images and their corresponding target detection information for the corresponding time moments are published at a specific frequency for subsequent camera pose optimization, thereby improving optimization efficiency. For example, the image acquisition frequency is 20 frames per second, and the publication frequency is 15 frames per second.

[0056] S13: When there is a QR code target in the target detection information, the image is placed into the window according to the preset window size. The camera pose corresponding to each frame of the image in the window is estimated by the optimization method of epipolar geometry and reprojection error, and the pose of each camera is transformed into the origin coordinate system with the center of the QR code as the origin.

[0057] It should be noted that, based on the target type in the target detection information, the image of the detected QR code can be identified. When the preset window size is 15, the image of the detected QR code is used as the first frame, and a total of 15 frames are placed into the window in sequence for pure visual initialization.

[0058] The pure visual initialization is consistent with current mainstream visual SLAM, such as Vinmono and Orbslam. Through optimization methods for epipolar geometry and reprojection errors, the camera pose corresponding to each frame within the window is estimated, denoted as follows: Where v represents the visual coordinate system and k is the number of images in the window.

[0059] It is important to note that the coordinate system defined during visual initialization is the visual coordinate system, which takes the position of the camera's optical center corresponding to the first frame image within the window as the origin of the visual coordinate system and remains fixed when estimating the camera pose.

[0060] To facilitate subsequent camera pose optimization, other data preprocessing was performed concurrently with the pure visual initialization, including:

[0061] The 3D coordinates of the feature point in the first image in the camera coordinate system are obtained by triangulation. Based on the 3D coordinates of the feature point, the normalized planar coordinates and inverse depth of the feature point are obtained. The camera coordinate system is a coordinate system constructed with the optical center of the camera as the origin and changes with the movement of the camera position. The 3D coordinates are uniformly represented in the form of (x, y, z). The normalized planar coordinates of the feature point are the coordinates when z is 1, that is, x and y are both divided by z, represented in the form of (u, v, 1). The inverse depth of the feature point is 1 / z in the 3D coordinates of the feature point.

[0062] Whenever a QR code target is detected in the target detection information, i.e., when a QR code is detected, the three-dimensional absolute coordinates of the corner points of the QR code are obtained with the center of the QR code as the origin and the size of the QR code in the loaded target information. Then, the P3P algorithm is used to calculate the camera prior pose corresponding to the image where the QR code is located.

[0063] Since the QR code's position remains fixed indoors, to avoid scale shifts that could lead to an infinite amplification of accumulated errors, this embodiment aligns the visual coordinate system with the origin coordinate system. This involves transforming the camera pose corresponding to each frame within the window from the visual coordinate system to the origin coordinate system with the QR code's center as the origin. This includes:

[0064] ①Based on the camera prior poses corresponding to the first and last images with detected QR codes in the window, the first relative pose, i.e. the relative pose with scale information, is obtained using the following formula;

[0065]

[0066] Where w represents the origin coordinate system, The camera prior pose corresponding to the first image in the window that detects a QR code; The camera prior pose corresponding to the last image in the window that detected the QR code; and For rotation and translation in relative pose with scale information.

[0067] ②Based on the estimated camera poses corresponding to the first and last images with detected QR codes in the window, the second relative pose is obtained using the following formula, which is the relative pose without absolute scale in the visual coordinate system.

[0068]

[0069] Where v represents the visual coordinate system, The camera pose estimated during the pure visual initialization process for the first image in the window that detects a QR code. The camera pose estimated during the pure visual initialization process for the last image in the window to detect a QR code. and For rotation and translation in relative pose without absolute scale.

[0070] ③ The ratio of the translation in the first relative pose to the translation in the second relative pose is used as a scale factor. The translation in each camera pose is multiplied by the scale factor to obtain the camera pose with absolute scale.

[0071] Specifically, the scaling factor is obtained using the following formula:

[0072]

[0073] Multiplying the translations in each camera pose by a scale factor yields the camera poses with absolute scale, expressed as follows:

[0074] ④ Based on the prior camera pose and the camera pose with absolute scale corresponding to the first image in the window that detects the QR code, obtain the coordinate transformation matrix, and then multiply it by the camera pose with absolute scale to obtain the pose of each camera in the origin coordinate system.

[0075] Specifically, the coordinate transformation matrix is ​​obtained using the following formula:

[0076]

[0077] in, and The camera prior pose corresponding to the first image within the window that detects a QR code. Rotation and translation; and The camera pose with absolute scale corresponding to the first image within the window that detects a QR code. Rotation and translation.

[0078] The coordinate transformation matrix is ​​then compared with the camera pose, which has an absolute scale. Multiplying these yields the poses of each camera in the origin coordinate system, represented as: T w1 ,T w2 ,…,T wk .

[0079] S14: Acquire a new image frame and begin camera pose optimization processing, including: using a uniform velocity model to predict the camera pose corresponding to the new image; for each image frame and the new image within the window, establish error functions based on the prior camera pose, camera pose, QR code corner coordinates, inverse depth of feature points, relative poses of adjacent frames, and the height of the gate; solve the solution with the minimum sum of squared errors by combining the error functions; obtain the optimized camera pose and inverse depth of feature points corresponding to each image frame and the new image within the window; slide the window to acquire another new image and repeat the camera pose optimization processing.

[0080] It should be noted that the camera pose optimization process involves, with each window slide,, constructing and simultaneously solving multiple error functions based on the information from each frame within the window. The goal is to minimize the sum of squared errors, transforming the problem into a least-squares problem. This yields the optimized camera pose and inverse depth of feature points. After each window slide, this process is iteratively applied to the next camera pose optimization. The camera pose is the one transformed to the origin coordinate system.

[0081] Since each frame in the initial window has been initialized in step S13 to obtain the camera pose, it is only necessary to predict the camera pose of the newly acquired frame. This embodiment uses a 3D-2D uniform motion model to predict the camera pose of the new image based on the camera pose of the last frame in the window and the transformation from the last frame to the current new image under the assumption of uniform motion.

[0082] For example, the camera pose in the 14th frame within the window is T. w14 The camera pose in frame 15 is T. w15 Then the relative pose between frame 14 and frame 15 is That is, the transformation from frame 15 to the current new image, then the predicted camera pose T of the current new image w_new =T w15 T 14_15 .

[0083] It should be noted that the newly acquired image frame is not placed in the window before optimization. The window holds 15 images. Therefore, each image frame in the current window and the new image are used as the current image to be optimized. Based on the information of the current image to be optimized, including: the camera prior pose and camera pose, the coordinates of the QR code corner points, the inverse depth of the feature points, the relative poses of adjacent frames, and the height of the gate, error functions are established respectively. The error functions include: prior error function, reprojection error function, inverse projection error function, observation error function, and gate height error function.

[0084] Specifically, ① an error function is established based on the prior camera pose and the current camera pose. This involves establishing the prior error function based on the prior camera pose corresponding to the detected QR code image, including:

[0085] Acquire images of detected QR codes and add them to a QR code image collection;

[0086] Each frame of the QR code image set is extracted sequentially. Based on the camera's prior pose and current camera pose, the rotation and translation errors are obtained. The prior error function is established using the following formula:

[0087]

[0088] Where e1 is the prior error function established for the current i-th frame image, and r Ri Let r be the rotation error of the current i-th frame image. ti Let be the translation error of the current i-th frame image. and Let R be the rotation and translation in the camera's prior pose corresponding to the current i-th frame image. wi and t wi Let R be the rotation and translation in the camera pose corresponding to the current i-th frame image; in the prior error function, R wi and t wi The state variable to be optimized.

[0089] ② Establishing error functions based on QR code corner coordinates involves comparing the normalized planar coordinates of the three-dimensional absolute coordinate projections of each corner point in the detected QR code image with the observed normalized planar coordinates of each corner point. This results in a reprojection error function for each corner point, including:

[0090] Each frame of the QR code image is extracted sequentially from the set of QR code images. Based on the three-dimensional absolute coordinates of the four corner points of the QR code in the current image and the camera pose corresponding to the current image, the three-dimensional coordinates of each corner point in the camera coordinate system are obtained by projection, as shown in the following formula:

[0091]

[0092] Among them, (x wi ,y wi ,z wi Let T be the three-dimensional absolute coordinates of a corner point of the QR code in the i-th frame of the image. wi Let R be the camera pose corresponding to the i-th frame image. wi and t wi For T wi Rotation and translation in; in the reprojection error function, R wi and t wi The state variable to be optimized.

[0093] Based on the pixel coordinates of each corner point, the normalized plane coordinates of each corner point are calculated by the camera model and used as the normalized plane coordinates of each corner point of the observed QR code.

[0094] Based on the 3D coordinates of each corner point in the camera coordinate system, the normalized planar coordinates of the projection of each corner point are obtained. These normalized planar coordinates are then compared with the observed normalized planar coordinates of each corner point of the QR code. The following formula is used to establish the reprojection error function:

[0095]

[0096] Among them, (u mi ,v mi ,1) represents the normalized planar coordinates of a corner point of the QR code observed in the i-th frame image.

[0097] ③ The error function is established based on the inverse depth of feature points. This involves taking a set of images with the same feature point number and, based on the normalized planar coordinates and inverse depth of the feature points in the camera coordinate system, projecting the normalized planar coordinates of the projected points from the image where the feature points were first observed onto any other frame in the set. These projections are then compared with the normalized planar coordinates of the feature points observed in another frame to establish the inverse projection error function, which includes:

[0098] Images with the same feature point number are obtained and placed into the feature point set corresponding to that feature point;

[0099] For each set of feature points, based on the normalized planar coordinates and inverse depth of the current feature point in the camera coordinate system, project the image from the first observed feature point onto any other frame in the set, and obtain the three-dimensional coordinates of the projected point in the camera coordinate system using the following formula:

[0100]

[0101] Where λ is the inverse depth of the feature point in the camera coordinate system, (u ti ,v ti ,1) represents the normalized planar coordinates of the feature point in the image where the current feature point is first observed; T wti Let R be the camera pose corresponding to the ti-th frame image. wti and t wti For T wti Rotation and translation in T wtj Let R be the camera pose corresponding to the tjth frame image. wtj and t wtj For T wtj Rotation and translation in the equation; in the inverse projection error function, R wti t wti R wtj t wtj Let λ and λ be the state variables to be optimized.

[0102] It should be noted that when the first observed i-th frame image of the current feature point is an image within the window, the normalized planar coordinates and inverse depth of each feature point in the camera coordinate system have already been obtained during the initialization process. When the first observed i-th frame image of the current feature point is a new image, for feature points that do not have three-dimensional coordinates, the three-dimensional coordinates of the feature points in the camera coordinate system are obtained through triangulation, and then the normalized planar coordinates and inverse depth of the feature points are obtained based on the three-dimensional coordinates.

[0103] Based on the 3D coordinates of the projected points in the camera coordinate system, the normalized planar coordinates are obtained. These, along with the normalized planar coordinates of the feature points observed in another frame, are used to establish the inverse projection error function using the following formula:

[0104]

[0105] Among them, (u tj ,v tj ,1) represents the normalized coordinates of the feature points observed in the tj-th frame image. These are the normalized planar coordinates calculated by the camera model based on the pixel coordinates of the feature points in the tj-th frame image.

[0106] This error function, based on the relative error constructed by object detection, constrains the motion between two frames, effectively avoiding the scale drift problem.

[0107] ④ Establishing an error function based on the relative pose of adjacent frames involves calculating the relative observation pose of adjacent frames with the same bounding box number and whose targets are QR codes or fire hydrants, based on the same target. This relative observation pose is then compared with the relative pose obtained from the camera pose of the adjacent frames to establish an observation error function, including:

[0108] Each adjacent frame is sequentially retrieved, and it is identified whether there are identical target bounding box numbers and whether the target is a QR code or a fire hydrant. If so, the target with the largest bounding box area is selected as the target to be calculated. The P3P algorithm is used to calculate the relative pose between the camera pose and the target in each adjacent frame, and then the relative observation pose between adjacent frames is calculated, as shown below:

[0109]

[0110] Among them, T p_obj T represents the relative pose of the camera and the target obj corresponding to the p-th frame image in an adjacent frame. q_obj The relative pose of the camera and the target obj is the image corresponding to the qth frame in an adjacent frame. and Relative observation pose of adjacent frames Rotation and translation within.

[0111] It should be noted that selecting the landmark with the largest area among the common landmarks helps improve calculation accuracy.

[0112] The relative pose obtained from the camera poses of adjacent frames is shown below:

[0113]

[0114] The observation error function is established using the following formula:

[0115]

[0116] Where, r Rpq For rotational error, r tpq For translation error; in the observation error function, the rotation R in the camera pose of adjacent frames. wp R wq Peaceful movement wp t wq The state variable to be optimized.

[0117] ⑤ Establishing an error function based on the door height involves converting the coordinates of any corner point on either side of the detected door from the origin coordinate system to three-dimensional coordinates in the camera coordinate system, calculating the door height, and comparing it with the actual door height corresponding to the target in the loaded target information to establish a door height error function, including:

[0118] Acquire images of detected doors and add them to a door image set;

[0119] Each frame is sequentially extracted from the door image set. Based on the camera pose of the current image, the coordinates of any corner point on either side of the door in the origin coordinate system are converted to three-dimensional coordinates in the camera coordinate system, as shown below:

[0120]

[0121]

[0122] in, and Let T be the coordinates of the top left and bottom left corners of the door in the l-th frame image, respectively, in the origin coordinate system, or the coordinates of the top right and bottom right corners of the door in the origin coordinate system. wl R represents the camera pose corresponding to the l-th frame image. wl Indicates rotation, t wl Represents translation; in the door height error function, R wl and t wl The state variable to be optimized.

[0123] The height of the door is calculated using the following formula based on the three-dimensional coordinates converted to the camera coordinate system:

[0124]

[0125] Get the actual height of the door corresponding to the target from the loaded target information. Use the following formula to establish the door height error:

[0126]

[0127] Finally, based on the above five error functions, and considering each frame of the image in the current window and the newly acquired image, we establish each error function and combine them using the following formula to obtain the total error function, as shown below:

[0128]

[0129] in, B is the collective term for the state variables to be optimized in each error function; B is the set of images of the observed QR code. Let C be the prior camera pose corresponding to the ei-th frame in the observed QR code image set; C is the set of observed corner points of the QR code. Let ej be the projection of the observed QR code's j-th corner point onto the normalized plane; D is the set of feature points for inverse projection point pairs, and (et, em) is a single inverse projection point pair in the set. Let be the projection of the ith feature point onto the normalized plane; E is the set of adjacent frames of the observed target. The relative observation pose is calculated for adjacent frames that observe the same target; Q is the set of observed corner points of the gate. The distance between the two corner points of the door; ||.|| 2 P represents the 2-norm. eiP ej P tm P ep and This represents the covariance matrix of the errors in each error function.

[0130] With the goal of minimizing the sum of squared errors, an overall least squares problem is constructed. The camera pose and inverse depth of feature points corresponding to each frame image in the window and the new image are optimized. After the current window is optimized, the camera pose and inverse depth of feature points corresponding to each frame image in the window and the new image are updated to the optimized camera pose and inverse depth of feature points. The window is slid with a step size of 1, including: removing the first frame image in the window, putting the new image into the slid window as the image in the slid window, and then continuing to acquire a new frame image. The camera pose optimization processing of this step is performed on each frame image in the window and the new image until the camera pose corresponding to each frame image is optimized.

[0131] For example, when the window size is 15, there are 15 frames in the initial window. A new frame, namely the 16th frame, is acquired. After the camera pose and inverse depth of feature points corresponding to these 16 frames are optimized, the 1st frame is removed and the 16th frame is placed into the sliding window. At this time, the camera pose and inverse depth of feature points corresponding to the 2nd to 16th frames in the window, as well as the acquired 17th frame, are optimized for the second round. After optimization, the 2nd frame is removed and the 17th frame is placed into the sliding window. At this time, the camera pose and inverse depth of feature points corresponding to the 3rd to 17th frames in the window, as well as the acquired 18th frame, are optimized for the third round. The camera pose and inverse depth of feature points corresponding to each frame in the window are iteratively optimized multiple times until the camera pose and inverse depth of feature points corresponding to each frame are optimized.

[0132] Compared with existing technologies, the monocular visual localization method for indoor office scenes provided in this embodiment utilizes information from fire hydrants, doors, and QR codes in the indoor scene to achieve SLAM localization of the indoor scene without increasing hardware costs. By combining monocular vision with deep learning object detection, it solves the scale uncertainty problem inherent in monocular vision. Simultaneously, the relative error constructed through object detection constrains motion between two frames, effectively avoiding scale drift. By adding absolute coordinates using QR codes, in large-scale scenes, the camera pose can be corrected in a timely manner when a QR code is seen, avoiding the problem of infinite amplification of accumulated errors. This invention avoids both the scale uncertainty and scale drift problems inherent in monocular SLAM and improves localization accuracy.

[0133] Example 2

[0134] Another embodiment of the present invention discloses a visual positioning system for a monocular indoor office scene, thereby implementing the visual positioning method in Embodiment 1. The specific implementation of each module is described in the corresponding description in Embodiment 1. The system includes:

[0135] The target detection module is used to load target information, perform target detection on each frame of image acquired in real time, and obtain target detection results; the targets include: 1 QR code, at least 1 fire hydrant, and a door;

[0136] The target tracking module is used to track the target detection results and obtain the feature point information and target detection information of each frame of image;

[0137] The visual initialization module is used to start placing image frames into the window according to the preset window size when there is a QR code target in the target detection information. Through the optimization method of epipolar geometry and reprojection error, the camera pose corresponding to each frame of the image in the window is estimated, and the camera poses are transformed into the origin coordinate system with the center of the QR code as the origin.

[0138] The pose optimization module is used to acquire a new image frame and start camera pose optimization processing, including: using a uniform velocity model to predict the camera pose corresponding to the new image; for each image frame and the new image within the window, establishing error functions based on the prior camera pose, camera pose, QR code corner coordinates, inverse depth of feature points, relative poses of adjacent frames, and the height of the gate; solving the solution with the minimum sum of squared errors by combining the error functions; obtaining the optimized camera pose and inverse depth of feature points corresponding to each image frame and the new image within the window; sliding the window to acquire another new image and repeating the camera pose optimization processing.

[0139] Since the visual positioning system for a monocular indoor office scene in this embodiment can be referenced from the aforementioned visual positioning method in some aspects, and this is a repetition, it will not be repeated here. Because this system embodiment shares the same principle as the above-described method embodiment, it also possesses the corresponding technical effects of the above-described method embodiment.

[0140] Those skilled in the art will understand that all or part of the processes of the methods described in the above embodiments can be implemented by a computer program instructing related hardware, and the program can be stored in a computer-readable storage medium. The computer-readable storage medium may be a disk, optical disk, read-only memory, or random access memory, etc.

[0141] The above description is only a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in the present invention should be included within the scope of protection of the present invention.

Claims

1. A visual positioning method for indoor office scenes based on monocular vision, characterized in that, Includes the following steps: Load target information, perform target detection on each frame of image acquired in real time, and obtain target detection results; the target includes: 1 QR code, at least 1 fire hydrant, and a door; Track each frame of the image to obtain the feature point information and target detection information of each frame; When there is a QR code target in the target detection information, the image is placed into the window according to the preset window size. Through the optimization method of epipolar geometry and reprojection error, the camera pose corresponding to each frame of the image in the window is estimated, and the camera pose is transformed into the origin coordinate system with the center of the QR code as the origin. To acquire a new image frame, camera pose optimization processing is initiated, including: using a uniform velocity model to predict the camera pose corresponding to the new image; for each image frame and the new image within the window, establishing error functions based on the prior camera pose, camera pose, QR code corner coordinates, inverse depth of feature points, relative poses of adjacent frames, and the height of the gate; simultaneously solving the system of error functions to calculate the solution with the minimum sum of squared errors; obtaining the optimized camera pose and inverse depth of feature points corresponding to each image frame and the new image within the window; sliding the window to acquire another new image; and repeating the camera pose optimization processing.

2. The visual positioning method for indoor office scenes based on monocular vision according to claim 1, characterized in that, The tracking of each frame of image yields feature point information and target detection information for each frame, including: The feature points of each frame of the image are obtained, and the feature points of each frame of the image are tracked using the optical flow method to obtain the feature point information, including: feature point pixel coordinates and feature point number. Among them, the feature points that are successfully tracked have the same feature point number. Based on the successfully tracked feature points, obtain the average movement vector of the feature points in the target box in the previous frame and the current frame, and based on the average movement vector, obtain the target box in the current frame in the previous frame, which is used as the first target box. When the intersection-union ratio (IUU) of the first target bounding box and the target bounding box in the previous frame is greater than the threshold, the target bounding box in the current frame is successfully associated with the target bounding box in the previous frame. The same target bounding box number is set and added to the target detection result to obtain the target detection information.

3. The visual positioning method for indoor office scenes based on monocular vision according to claim 2, characterized in that, When there is a QR code target in the target detection information, the method further includes: taking the center of the QR code as the origin, obtaining the three-dimensional absolute coordinates of the corner points of the QR code according to the size of the QR code in the loaded target information, and using the P3P algorithm to calculate the camera prior pose corresponding to the image where the QR code is located. The step of placing the image into the window according to the preset window size also includes: obtaining the three-dimensional coordinates of the feature points in the first appearance window image in the camera coordinate system through a triangulation method, and obtaining the normalized planar coordinates and inverse depth of the feature points based on the three-dimensional coordinates of the feature points.

4. The visual positioning method for indoor office scenes based on monocular vision according to claim 3, characterized in that, The process of converting the poses of each camera to the origin coordinate system with the center of the QR code as the origin includes: The first relative pose is obtained based on the camera prior poses corresponding to the first and last images with detected QR codes in the window. The second relative pose is obtained based on the estimated camera poses corresponding to the first and last images of the detected QR codes within the window; The ratio of the translation in the first relative pose to the translation in the second relative pose is used as a scale factor. The translation in each camera pose is multiplied by the scale factor to obtain the camera pose with absolute scale. Based on the prior camera pose and the pose with absolute scale corresponding to the first image in the window that detects the QR code, the coordinate transformation matrix is ​​obtained. Then, it is multiplied by the camera pose with absolute scale to obtain the pose of each camera in the origin coordinate system.

5. The visual positioning method for indoor office scenes based on monocular vision according to claim 4, characterized in that, The step of establishing an error function based on the prior camera pose and the current camera pose involves establishing the prior error function based on the prior camera pose corresponding to the image of the detected QR code and the corresponding current camera pose.

6. The visual positioning method for indoor office scenes based on monocular vision according to claim 4, characterized in that, The step of establishing the error function based on the corner coordinates of the QR code involves taking the image of the detected QR code and, respectively, establishing the reprojection error function for each corner point based on the normalized plane coordinates of the three-dimensional absolute coordinate projection of each corner point of the QR code and the observed normalized plane coordinates of each corner point of the QR code.

7. The visual positioning method for indoor office scenes based on monocular vision according to claim 4, characterized in that, The step of establishing an error function based on the inverse depth of feature points involves taking a set of images with the same feature point number, and using the normalized planar coordinates and inverse depth of the feature points in the camera coordinate system, projecting the normalized planar coordinates of the projected points from the image where the feature points were first observed onto any other frame in the set. This normalized planar coordinates are then compared with the normalized planar coordinates of the feature points observed in another frame to establish an inverse projection error function.

8. The visual positioning method for indoor office scenes based on monocular vision according to claim 4, characterized in that, The step of establishing an error function based on the relative pose of adjacent frames involves calculating the relative observation pose of adjacent frames with the same target box number and whose target is a QR code or a fire hydrant, based on the same target, and then establishing an observation error function with the relative pose obtained from the camera pose of the adjacent frames.

9. The visual positioning method for indoor office scenes based on monocular vision according to claim 4, characterized in that, The step of establishing an error function based on the door height involves taking the image of the detected door, converting the coordinates of any corner point on either side of the door in the origin coordinate system to three-dimensional coordinates in the camera coordinate system, calculating the door height, and comparing it with the actual door height corresponding to the target in the loaded target information to establish a door height error function.

10. A monocular visual positioning system for indoor office scenes, characterized in that, include: The target detection module is used to load target information, perform target detection on each frame of image acquired in real time, and obtain target detection results; the target includes: one QR code, at least one fire hydrant, and a door; The target tracking module is used to track the target detection results and obtain the feature point information and target detection information of each frame of image; The visual initialization module is used to start placing image frames into the window according to the preset window size when there is a QR code target in the target detection information. Through the optimization method of epipolar geometry and reprojection error, the camera pose corresponding to each frame of the image in the window is estimated, and the camera poses are transformed into the origin coordinate system with the center of the QR code as the origin. The pose optimization module is used to acquire a new image frame and start camera pose optimization processing, including: predicting the camera pose corresponding to the new image using a uniform velocity model; establishing error functions for each image frame and the new image within the window based on the prior camera pose, camera pose, QR code corner coordinates, inverse depth of feature points, relative poses of adjacent frames, and the height of the gate; calculating the solution with the minimum sum of squared errors by combining the error functions; obtaining the optimized camera pose and inverse depth of feature points corresponding to each image frame and the new image within the window; sliding the window to acquire a new image frame; and repeating the camera pose optimization processing.

Images