Target direction detection method and device, electronic equipment and storage medium
By combining image and lidar technology, the lidar point cloud in the target local plane area is intercepted and plane fitted, the problem of direction determination under lidar coordinates is solved, and accurate direction detection and low-cost hardware structure are achieved.
Patent Information
- Application Number
- CN202411809489.7
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-12-10
- Publication Date
- 2025-05-16
AI Technical Summary
The prior art cannot accurately determine the orientation of the object/object under lidar coordinates, especially in scenarios where precise piling is required.
By combining image and lidar technology, the lidar point cloud of the designated target's local plane area is intercepted and plane fitting is performed to calculate the angle with the specified direction to determine the direction of the target in the lidar coordinate system.
It realizes accurate detection of the target's direction under the lidar coordinates, is suitable for a variety of scenarios, and has a relatively simple hardware structure and low cost.
Smart Images
Figure CN120014033A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of laser radar detection technology, and in particular to a method, device, electronic equipment and storage medium for detecting the direction of a target. Background Art
[0002] At present, LiDAR has been widely used in many fields, such as unmanned driving, robots, and metaverse mapping. LiDAR mainly obtains a three-dimensional point cloud in three-dimensional space by emitting lasers and measuring their reflection time. Each point in the three-dimensional point cloud represents a spatial position. The distance and shape of objects around the LiDAR can be determined through the three-dimensional point cloud, which can facilitate object detection, obstacle avoidance, map construction, etc.
[0003] Generally, the three-dimensional point cloud of the lidar can give the position coordinates of an object in the lidar coordinate system, which can intuitively calculate the distance from a local point of a specified object to the lidar. However, in many application scenarios, such as the precise charging pile scenario of the robot, it is required not only to obtain the distance between the charging pile and the robot, but also to further accurately obtain the direction of the charging pile in the lidar coordinate system.
[0004] Based on this, how to determine the direction of the object / subject in the laser radar coordinates becomes a technical problem that needs to be solved urgently. Summary of the invention
[0005] The present invention provides a target direction detection method, device, electronic device and storage medium, which are used to solve the defect in the prior art that the direction of a target such as an object cannot be determined in a laser radar coordinate system, and realizes the purpose of intercepting the laser radar point cloud of a local plane area of a specified target by combining an image and a laser radar and performing plane fitting, and then calculating the angle with the specified direction to obtain the direction of the target in the laser radar coordinate system, and has a wider range of applicability.
[0006] The present invention provides a method for detecting the direction of a target, comprising: Acquire the image to be tested captured by the camera installed on the robot, and segment the target in the image to be tested to determine the target segmentation mask corresponding to the image to be tested; the target segmentation mask includes the target; Acquire three-dimensional point cloud data collected by the laser radar installed on the robot; the three-dimensional point cloud data is collected at the same time as the image to be measured; According to the coordinate system transformation parameters between the laser radar and the camera and the position information of the target in the target segmentation mask, the target 3D point cloud data corresponding to the target is intercepted from the 3D point cloud data; The target 3D point cloud data is processed by plane fitting to obtain the laser point cloud fitting plane, and the direction of the target in the laser radar coordinates is determined according to the normal vector of the laser point cloud fitting plane.
[0007] According to a method for detecting the direction of a target provided by the present invention, before determining the direction of the target in the laser radar coordinates according to the normal vector of the plane fitted by the laser point cloud, the method further includes: Determine whether the target 3D point cloud data is valid; If invalid, return to re-execute the above step of intercepting the target 3D point cloud data corresponding to the target from the 3D point cloud data according to the coordinate system transformation parameters between the laser radar and the camera and the position information of the target in the target segmentation mask, and obtain new target 3D point cloud data; If valid, execute the above step of determining the direction of the target in the laser radar coordinates according to the normal vector of the laser point cloud fitting plane.
[0008] According to a target direction detection method provided by the present invention, the above-mentioned determination of whether the target three-dimensional point cloud data is valid includes: Obtain the three-dimensional coordinates of each point in the target three-dimensional point cloud data and the plane equation corresponding to the laser point cloud fitting plane; According to the three-dimensional coordinates and plane equations of each point, calculate the distance from each point to the laser point cloud fitting plane; Whether the target 3D point cloud data is valid is determined based on each distance.
[0009] According to a target direction detection method provided by the present invention, the above-mentioned determining whether the target three-dimensional point cloud data is valid according to each distance includes: Calculate the mean and variance corresponding to each distance; If the mean exceeds the first threshold, and / or the variance exceeds the second threshold, it is determined that the target three-dimensional point cloud data is invalid.
[0010] According to a target direction detection method provided by the present invention, the above-mentioned determination of the target direction in laser radar coordinates based on the normal vector of the laser point cloud fitting plane includes: Obtaining the plane equation corresponding to the laser point cloud fitting plane, and determining the normal vector corresponding to the laser point cloud fitting plane according to the plane equation; Obtain a preset reference vector; The direction of the target in the LiDAR coordinates is determined based on the angle between the normal vector and the reference vector.
[0011] According to a target direction detection method provided by the present invention, the above-mentioned plane fitting processing is performed on the target three-dimensional point cloud data to obtain the laser point cloud fitting plane, including: Obtain the three-dimensional coordinates of each point in the target three-dimensional point cloud data, and calculate the three-dimensional coordinates of the center of mass corresponding to the target three-dimensional point cloud data according to the three-dimensional coordinates of each point; According to the three-dimensional coordinates of the centroid, the three-dimensional coordinates of each point are de-centroided to determine the three-dimensional coordinates of each point after de-centroiding; Construct a characteristic matrix based on the de-centroided three-dimensional coordinates of each point, and solve the characteristic matrix to determine the coefficients of the plane equation; The corresponding plane equation is constructed according to the plane equation coefficients to determine the laser point cloud fitting plane.
[0012] According to a target direction detection method provided by the present invention, the target three-dimensional point cloud data corresponding to the target is intercepted from the three-dimensional point cloud data according to the coordinate system transformation parameters between the laser radar and the camera and the position information of the target in the target segmentation mask, including: According to the coordinate system transformation parameters between the laser radar and the camera, the position information of each point in the 3D point cloud data is converted from the laser radar coordinates to the image coordinate system corresponding to the target segmentation mask, and the corresponding position information of each point in the image coordinates is determined; Selecting the position information corresponding to the target in the target segmentation mask from the position information corresponding to each point in the image coordinates; According to the index corresponding to the position information of the selected target in the image coordinate system, the point cloud data corresponding to the index is intercepted in the three-dimensional point cloud data to obtain the target three-dimensional point cloud data corresponding to the target.
[0013] The present invention also provides a target direction detection device, comprising the following modules: An image acquisition and segmentation module is used to acquire the image to be tested captured by the camera installed on the robot, and to segment the target in the image to be tested, and determine the target segmentation mask corresponding to the image to be tested; the target segmentation mask includes the target; The original point cloud acquisition module is used to acquire the three-dimensional point cloud data collected by the laser radar installed on the robot; the three-dimensional point cloud data is collected at the same time as the image to be measured; The target point cloud acquisition module is used to intercept the target 3D point cloud data corresponding to the target from the 3D point cloud data according to the coordinate system transformation parameters between the laser radar and the camera and the position information of the target in the target segmentation mask; The direction detection module is used to perform plane fitting processing on the target three-dimensional point cloud data, obtain the laser point cloud fitting plane, and determine the direction of the target in the laser radar coordinates according to the normal vector of the laser point cloud fitting plane.
[0014] The present invention also provides an electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the computer program, a target direction detection method as described above is implemented.
[0015] The present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, and when the computer program is executed by a processor, the method for detecting the direction of a target as described above is implemented.
[0016] The present invention also provides a computer program product, comprising a computer program, wherein when the computer program is executed by a processor, the method for detecting the direction of a target as described above is implemented.
[0017] The target direction detection method, device, electronic device and storage medium provided by the present invention obtain the image to be tested captured by a camera installed on the robot, and segment the target in the image to be tested to determine a target segmentation mask including the target corresponding to the image to be tested, and simultaneously obtain the three-dimensional point cloud data collected by the laser radar installed on the robot at the same time, and then according to the coordinate system transformation parameters between the laser radar and the camera and the position information of the target in the target segmentation mask, the target three-dimensional point cloud data corresponding to the target is intercepted from the three-dimensional point cloud data, and then the target three-dimensional point cloud data is plane-fitted to obtain the laser point cloud fitting plane, and the direction of the target in the laser radar coordinate system is determined according to the normal vector of the laser point cloud fitting plane. In this method, since the lidar point cloud of the local plane area of the specified target can be intercepted and the plane can be fitted by combining images and lidar, and then the direction of the target in the lidar coordinate system can be determined by fitting the normal vector of the plane, it is possible to accurately detect the direction of the target in the lidar coordinates and provide a data basis for other subsequent applications; at the same time, the hardware structure for implementing this process is relatively simple, so low-cost lidar point cloud direction detection can be achieved, and it can be applicable in various scenarios, that is, the scope of application is relatively wide. BRIEF DESCRIPTION OF THE DRAWINGS
[0018] In order to more clearly illustrate the technical solutions in the present invention or the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.
[0019] Figure 1 This is one of the flow charts of the target direction detection method provided by the present invention.
[0020] Figure 2 It is a schematic diagram of the structure of a robot provided by the present invention equipped with a camera and a laser radar.
[0021] Figure 3 It is a schematic diagram of the image to be measured and the target provided by the present invention.
[0022] Figure 4It is a schematic diagram of the three-dimensional point cloud data and the target three-dimensional point cloud data provided by the present invention.
[0023] Figure 5 It is a schematic diagram of target three-dimensional point cloud data provided by the present invention.
[0024] Figure 6 It is a plane schematic diagram of laser point cloud fitting provided by the present invention.
[0025] Figure 7 This is the second flow chart of the target direction detection method provided by the present invention.
[0026] Figure 8 It is a schematic diagram of the distance between each point in the target three-dimensional point cloud data provided by the present invention and the laser point cloud fitting plane.
[0027] Fig. 9 This is the third flow chart of the target direction detection method provided by the present invention.
[0028] Fig.10 This is the fourth flow chart of the target direction detection method provided by the present invention.
[0029] Fig.11 It is a schematic diagram of the direction of the target provided by the present invention under the laser radar coordinates.
[0030] Fig.12 It is a structural schematic diagram of a target direction detection device provided by the present invention.
[0031] Fig.13 It is a structural schematic diagram of the electronic device provided by the present invention. DETAILED DESCRIPTION
[0032] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution of the present invention will be clearly and completely described below in conjunction with the drawings of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.
[0033] At present, when determining the direction of a target such as an object, there is a solution for determining the direction by infrared detection. However, this solution requires gradually moving a mobile device such as a robot during measurement, and repeatedly performing infrared detection to determine the direction of the target. This process is cumbersome and time-consuming. At the same time, the infrared detection method cannot be used in some scenarios, which will limit the use of the infrared detection direction. Based on this, an embodiment of the present invention provides a target direction detection method, device, electronic device and storage medium, which can solve this technical problem.
[0034] Combine the following Figure 1-Figure 11 A method for detecting the direction of a target according to an embodiment of the present invention is described.
[0035] It should be noted that the execution subject of the embodiment of the present invention can be a target direction detection device, or an electronic device, or other devices or apparatuses, which are not specifically limited here. The following embodiments are described by taking the electronic device as the execution subject. It is understandable that the electronic device can be a robot or an electronic device in a robot, or a laser radar or an electronic device in a laser radar, or a camera or an electronic device in a camera, or a background server that communicates with a robot / camera / laser radar, etc.
[0036] Figure 1 FIG. 1 is one of the flow charts of the target direction detection method provided by the present invention, such as Figure 1 As shown, the method comprises the following steps: Step 102, obtaining the image to be tested captured by the camera installed on the robot, and performing segmentation processing on the target in the image to be tested to determine the target segmentation mask corresponding to the image to be tested; the target segmentation mask includes the target.
[0037] The structure of the robot in the embodiment of the present invention is first described. Figure 2 The schematic diagram of the structure of the robot with camera and laser radar is shown in the figure. The camera and laser radar can be installed above the robot, where the camera is used to collect images of the scene and the laser radar is used to collect 3D point clouds of the scene. When installing, the laser radar can be installed above the camera, that is, the camera can be installed below the laser radar (such as Figure 2 ), or the camera can be installed above the laser radar, that is, the laser radar is installed below (not shown). This is only an example and is not specifically limited. After the camera and laser radar are installed on the robot, the camera can be calibrated or self-calibrated by looking up the pre-stored camera intrinsic parameters, and then the camera and laser radar can be jointly calibrated to determine the external parameter matrix between the camera and the laser radar. The external parameter matrix is an external parameter matrix that transforms the point cloud of the laser radar from the laser radar coordinate system to the camera coordinate system, and can also be called a coordinate system transformation parameter. The above-mentioned robot can be an automatically moving robot. In this embodiment, the robot needs to move to the target. In order to move accurately, it is necessary to know the direction of the target in the laser radar coordinate system. The following will explain the process of specifically determining the direction of the target in the laser radar coordinate system.
[0038] In this step, at the current moment, the camera on the robot can collect the image to be tested including the target in the current scene at the current position, and then segment the target in the image to be tested using the segmentation model constructed by the neural network model or other segmentation algorithms, and obtain the segmentation mask of the target, which is recorded as the target segmentation mask. The foreground of the target segmentation mask is the target, and the rest are the background. The target can be a selected landmark in the current scene, such as a desk, chair, building, roadblock, tree, etc.
[0039] In addition, when segmenting an object, you can segment only the local plane / surface of the object, or you can segment all the planes / surfaces of the object. Figure 3 The schematic diagram of the image to be tested and the target is shown, wherein the entire picture can be the image to be tested of the current scene captured by the camera, wherein the white dotted box area is the local plane of the target (table), that is, the foreground area included in the subsequent target segmentation mask.
[0040] Furthermore, when the target segmentation mask is obtained, the position information of each point on the target in the target segmentation mask can also be obtained. The position information is the position information of the target in the image coordinate system, which is generally a two-dimensional coordinate.
[0041] Step 104, obtaining three-dimensional point cloud data collected by the laser radar installed on the robot; the three-dimensional point cloud data is collected at the same time as the image to be measured.
[0042] In this step, the laser radar on the robot can also collect the three-dimensional point cloud of the target in the current scene while the camera collects the image to be tested, and obtain the three-dimensional point cloud data at the same time as the image to be tested is collected. It can be understood that the three-dimensional point cloud data includes multiple points and the position information of each point, wherein the position information of each point can be the three-dimensional coordinates of the point in the laser radar coordinate system.
[0043] Step 106 , based on the coordinate system transformation parameters between the laser radar and the camera and the position information of the target in the target segmentation mask, intercept the target three-dimensional point cloud data corresponding to the target from the three-dimensional point cloud data.
[0044] In this step, as mentioned above, after the laser radar and the camera are jointly calibrated, the coordinate system transformation parameters between the laser radar and the camera can be obtained. Then, as an optional embodiment, the position information of each point in the three-dimensional point cloud data (specifically, the three-dimensional coordinates of the point) can be converted from the laser radar coordinates to the image coordinate system corresponding to the target segmentation mask according to the coordinate system transformation parameters between the laser radar and the camera, and the corresponding position information of each point in the image coordinates can be determined. The above-mentioned coordinate system transformation parameters include the intrinsic parameter matrix of the camera and the extrinsic parameter matrix between the camera and the laser radar. Specifically, the above-mentioned coordinate transformation parameters can be used to first transform each point in the three-dimensional point cloud data from the laser radar coordinate system to the image coordinate system, that is, to obtain the position information of each point in the three-dimensional point cloud data in the image coordinate system (the image coordinate system is generally a two-dimensional coordinate).
[0045] Then, the position information corresponding to the target in the target segmentation mask can be selected from the position information corresponding to each point in the image coordinate system; according to the index corresponding to the position information of the selected target in the image coordinate system, the point cloud data corresponding to the index is intercepted in the three-dimensional point cloud data to obtain the target three-dimensional point cloud data corresponding to the target. That is to say, after the three-dimensional coordinates of each point in the three-dimensional point cloud data are transformed to the image coordinate system, the position information of the target in the target segmentation mask can be used to select the position information of the target in the transformed position information of the three-dimensional point cloud data, and the index of the position information of these selected targets in the transformed position information can be obtained. The index is the same as the index in the three-dimensional point cloud data, that is, the index of a point of a certain index in the three-dimensional point cloud data remains unchanged when it is transformed to the image coordinate system. Therefore, after selecting and determining the index corresponding to the target position information, the three-dimensional coordinates of the points with the same index can be found in the three-dimensional point cloud data. These points are the points corresponding to the target in the three-dimensional point cloud data. Then, these found points and their three-dimensional coordinates are combined to obtain the target three-dimensional point cloud data corresponding to the target. For example, see Figure 4 The schematic diagram of the three-dimensional point cloud data and the target three-dimensional point cloud data is shown, where the blue points are the overall three-dimensional point cloud data, and the red cross points are the target three-dimensional point cloud data of the target. In this way, the target three-dimensional point cloud data of the target can be intercepted from the three-dimensional point cloud data collected at the current moment.
[0046] Step 108, performing plane fitting processing on the target three-dimensional point cloud data to obtain a laser point cloud fitting plane, and determining the direction of the target in the laser radar coordinates according to the normal vector of the laser point cloud fitting plane.
[0047] After obtaining the target 3D point cloud data of the target, a plane fitting process can be performed based on the position information (3D coordinates) of each point in the target 3D point cloud data to obtain a fitting plane corresponding to the target 3D point cloud data, which is recorded as the laser point cloud fitting plane. It should be noted that the distance between the fitted laser point cloud fitting plane and the target 3D point cloud is more appropriate, and can better represent the target 3D point cloud. For example, see Figure 5 The target 3D point cloud data schematic diagram and Figure 6 The schematic diagram of the laser point cloud fitting plane is shown, where the black points are the target three-dimensional point cloud data and the gray plane is the fitted laser point cloud fitting plane.
[0048] After obtaining the laser point cloud fitting plane corresponding to the target three-dimensional point cloud data, the normal vector of the laser point cloud fitting plane can be directly calculated, and then the angle between the normal vector and the specified axis can be calculated, and the calculated angle can be used as the direction of the target in the laser radar coordinates; or the angle between the normal vector and any axis of the laser point cloud fitting plane can be calculated, and the calculated angle can be used as the direction of the target in the laser radar coordinates; or the direction of the target in the laser radar coordinates can be calculated by the normal vector of the laser point cloud fitting plane. In short, the direction of the target in the laser radar coordinates can be obtained.
[0049] In this embodiment, the image to be tested collected by the camera installed on the robot is obtained, and the target in the image to be tested is segmented to determine the target segmentation mask including the target corresponding to the image to be tested, and the three-dimensional point cloud data collected by the laser radar installed on the robot at the same time is obtained, and then the target three-dimensional point cloud data corresponding to the target is intercepted from the three-dimensional point cloud data according to the coordinate system transformation parameters between the laser radar and the camera and the position information of the target in the target segmentation mask, and then the target three-dimensional point cloud data is plane-fitted to obtain the laser point cloud fitting plane, and the direction of the target in the laser radar coordinate system is determined according to the normal vector of the laser point cloud fitting plane. In this method, since the laser radar point cloud of the local plane area of the specified target can be intercepted and plane-fitted by combining the image and the laser radar, and then the direction of the target in the laser radar coordinate system is determined by the normal vector of the fitting plane, the direction of the target in the laser radar coordinate system can be accurately detected, which provides a data basis for subsequent other applications; at the same time, the hardware structure of the implementation of this process is relatively simple, so low-cost laser radar point cloud direction detection can be achieved, and it can be applied in various scenarios, that is, the scope of application is relatively wide.
[0050] The following embodiment describes the process of determining the validity of laser point cloud data before determining the direction of the target in laser radar coordinates.
[0051] Figure 7FIG. 2 is a flow chart of the target direction detection method provided by the present invention. Figure 7 As shown, before "determining the direction of the target in the laser radar coordinates according to the normal vector of the laser point cloud fitting plane" in the above step 108, the above method also includes the following steps: Step 202, determining whether the target three-dimensional point cloud data is valid.
[0052] In this step, in order to ensure the accuracy of the target direction in the laser radar coordinates determined by the target 3D point cloud data, before determining the direction, it is possible to first determine whether the target 3D point cloud data is valid. The validity here can reflect whether the target 3D point cloud data is evenly distributed, whether the distance between the fitted laser point cloud fitting plane and the target 3D point cloud data is appropriate, etc.
[0053] As an optional embodiment, when specifically judging whether it is valid, the three-dimensional coordinates of each point in the target three-dimensional point cloud data and the plane equation corresponding to the laser point cloud fitting plane can be obtained; based on the three-dimensional coordinates and plane equation of each point, the distance from each point to the laser point cloud fitting plane is calculated; and based on each distance, it is determined whether the target three-dimensional point cloud data is valid.
[0054] Assume that the three-dimensional coordinates of the points in the target three-dimensional point cloud data can be expressed as (x i ,y i ,z i ), i represents the i-th point in the target 3D point cloud data. Assuming that the plane equation of the laser point cloud fitting plane can be expressed as , where a, b, c, and d are all coefficients of the plane equation. After obtaining the laser point cloud fitting plane, they are known values. Then the distance from each point in the target 3D point cloud data to the laser point cloud fitting plane can be calculated by the following formula: .
[0055] Among them, D i Indicates the distance from the i-th point in the target 3D point cloud data to the laser point cloud fitting plane. For example, see Figure 8 The diagram shows the distance between each point in the target three-dimensional point cloud data and the laser point cloud fitting plane, where the horizontal axis is the point in the target three-dimensional point cloud data and the vertical axis is the distance between each point and the laser point cloud fitting plane.
[0056] After calculating the distance from each point in the target three-dimensional point cloud data to the laser point cloud fitting plane, as an optional embodiment, the mean and variance corresponding to each distance can be calculated. That is, the distance corresponding to each point can be summed and averaged, and the obtained mean is the mean of the distances from each point to the laser point cloud fitting plane, and then the difference between the distance of each point and the mean can be calculated, and then the variance of the distance from each point to the laser point cloud fitting plane can be obtained by calculating the differences. After that, the calculated mean can be compared with the preset first threshold, and the calculated variance can be compared with the preset second threshold.
[0057] If the mean exceeds the first threshold, and / or the variance exceeds the second threshold, the target three-dimensional point cloud data is determined to be invalid, that is, if at least one of the calculated mean and variance exceeds their respective corresponding thresholds, it means that the distribution of the target three-dimensional point cloud data intercepted this time is not very uniform or the distance between the fitted laser point cloud fitting plane and the target three-dimensional point cloud data is not appropriate, etc., that is, the target three-dimensional point cloud data intercepted this time is invalid.
[0058] If the mean and variance do not exceed their respective thresholds, it means that the target 3D point cloud data intercepted this time is distributed relatively evenly or the distance between the fitted laser point cloud fitting plane and the target 3D point cloud data is relatively appropriate, that is, the target 3D point cloud data intercepted this time is valid.
[0059] The respective sizes of the first threshold and the second threshold can be set according to actual conditions and are not specifically limited here.
[0060] Step 204, if invalid, returns to re-execute the above step of intercepting the target three-dimensional point cloud data corresponding to the target from the three-dimensional point cloud data according to the coordinate system transformation parameters between the laser radar and the camera and the position information of the target in the target segmentation mask, and obtains new target three-dimensional point cloud data.
[0061] In this step, if it is determined that the target 3D point cloud data intercepted this time is invalid, the target 3D point cloud data intercepted this time is discarded, and then the target 3D point cloud data is re-intercepted. At this time, the above 106 can be returned to be re-executed, that is, according to the coordinate system transformation parameters between the laser radar and the camera and the position information of the target in the target segmentation mask, the target 3D point cloud data corresponding to the target is re-intercepted from the 3D point cloud data to obtain the new target 3D point cloud data intercepted this time. Then continue to perform plane fitting and determine the validity of the target 3D point cloud data until the target 3D point cloud data finally intercepted is valid or other cutoff conditions are reached.
[0062] Step 206, if valid, execute the above step of determining the direction of the target in the laser radar coordinates according to the normal vector of the laser point cloud fitting plane.
[0063] In this step, if it is determined that the target three-dimensional point cloud data intercepted this time is valid, the corresponding laser point cloud fitting plane can be obtained through the target three-dimensional point cloud data intercepted this time, and then the direction of the target in the lidar coordinates can be determined through the normal vector of the laser point cloud fitting plane.
[0064] In this embodiment, before determining the direction of the target in the laser radar coordinates through the target three-dimensional point cloud data and the fitted laser point cloud fitting plane, it is also possible to determine whether the intercepted target laser point cloud data is valid, and determine the direction if it is valid, or re-intercept the target three-dimensional point cloud data if it is invalid, so as to ensure the accuracy of the direction of the target in the laser radar coordinates finally determined by the target three-dimensional point cloud data. In addition, the validity is determined by the distance between each point in the intercepted target three-dimensional point cloud data and the fitting plane. This process is simple and intuitive, so the efficiency and accuracy of judging the validity of the data can be improved. Furthermore, when at least one of the mean and variance of the distance between each point in the target three-dimensional point cloud data and the fitting plane exceeds the threshold, the data is determined to be invalid, so that the accuracy of the target three-dimensional point cloud data finally intercepted can be improved, and the accuracy of the direction of the target finally determined therefrom can be further improved.
[0065] The following embodiment takes the three-dimensional coordinates of each point in the target three-dimensional point cloud data as an example to illustrate the process of specifically calculating the laser point cloud fitting plane of the target.
[0066] Fig. 9 FIG. 3 is a flow chart of the target direction detection method provided by the present invention. Fig. 9 As shown, in the above step 108, "performing plane fitting processing on the target three-dimensional point cloud data to obtain the laser point cloud fitting plane" may include the following steps: Step 302, obtaining the three-dimensional coordinates of each point in the target three-dimensional point cloud data, and calculating the three-dimensional coordinates of the centroid corresponding to the target three-dimensional point cloud data according to the three-dimensional coordinates of each point.
[0067] In this step, it is assumed that the three-dimensional coordinates of the points in the target three-dimensional point cloud data can be expressed as (x i ,y i ,z i ), i represents the i-th point in the target 3D point cloud data. Assuming there are n points in total, the 3D coordinates (x a ,y a ,z a ): .
[0068] Step 304 , de-centroiding the three-dimensional coordinates of each point according to the three-dimensional coordinates of the centroid, and determining the de-centroided three-dimensional coordinates of each point.
[0069] In this step, after obtaining the 3D coordinates of the centroid in the target 3D point cloud data, the 3D coordinates of each point in the target 3D point cloud data can be subtracted from the 3D coordinates of the centroid to obtain the 3D coordinates of each point after de-centroiding, which is recorded as (x r ,y r ,z r ), which can be specifically expressed by the following formula: .
[0070] Step 306, constructing a characteristic matrix according to the three-dimensional coordinates of each point after de-centroiding, and solving the characteristic matrix to determine the coefficients of the plane equation.
[0071] In this step, after obtaining the de-centroided 3D coordinates of each point in the target 3D point cloud data, the following 3×3 feature matrix M can be constructed: .
[0072] After constructing the characteristic matrix M, the eigenvalues and eigenvectors of the characteristic matrix M can be solved to obtain multiple eigenvalues and eigenvectors corresponding to each eigenvalue, each of which includes three elements. Then the smallest eigenvalue can be selected, and the values of the three elements included in the eigenvector corresponding to the smallest eigenvalue are used as the plane equation coefficients of the laser fitting plane. Assume that the plane equation of the laser point cloud fitting plane can be expressed as , then the values of the three elements included in the eigenvector corresponding to the smallest eigenvalue here can be used as the values of a, b, and c respectively.
[0073] The value of d in the plane equation can be calculated using the three-dimensional coordinates of the centroid and the values of a, b, and c determined above. Specifically, the following formula can be used for calculation: .
[0074] Step 308: construct a corresponding plane equation according to the plane equation coefficients to determine the laser point cloud fitting plane.
[0075] In this step, after the plane equation coefficients of the laser point cloud fitting plane are determined as above, each plane equation coefficient is substituted into the above plane equation to obtain a plane equation with known coefficients. The plane represented by the plane equation is the laser point cloud fitting plane.
[0076] In this embodiment, each point in the target three-dimensional point cloud data is de-centroided, and then a feature matrix is constructed based on the three-dimensional coordinates of the de-centroided points and the coefficients of the fitting plane equation are obtained by solving. This can reduce noise such as random errors caused by data drift, thereby improving the accuracy of the final determined fitting plane, and then improving the accuracy of the subsequent calculated direction. At the same time, by constructing a feature matrix and solving the coefficients of the fitting plane equation, the process is simple and intuitive, and therefore can improve the efficiency and accuracy of determining the fitting plane, and then improve the efficiency and accuracy of the subsequent calculated direction.
[0077] The following embodiment describes the process of calculating the direction of a target in laser radar coordinates.
[0078] Fig.10 FIG. 4 is a flow chart of the target direction detection method provided by the present invention. Fig.10 As shown, in the above step 108, "determining the direction of the target in the laser radar coordinates according to the normal vector of the laser point cloud fitting plane" may include the following steps: Step 402: Obtain a plane equation corresponding to the laser point cloud fitting plane, and determine a normal vector corresponding to the laser point cloud fitting plane according to the plane equation.
[0079] In this step, after the plane equation corresponding to the laser point cloud fitting plane is obtained by solving the characteristic matrix and substituting the coefficients, the plane equation Where a, b, c, and d are all known quantities, the normal vector of the laser point cloud fitting plane can be calculated by the plane equation, and the normal vector can be expressed as v = (a, b, c).
[0080] Step 404: Obtain a preset reference vector.
[0081] In this step, the preset reference vector can be a vector corresponding to any reference axis in the laser radar coordinate system, which can be determined by the user or the robot's default selection. For example, if the x-axis is the reference axis, the reference vector can be (1, 0, 0); if the y-axis is the reference axis, the reference vector can be (0, 1, 0); if the z-axis is the reference axis, the reference vector can be (0, 0, 1). Of course, the reference vector can also be a vector in a non-axial direction, and the reference vector can be selected according to the actual situation.
[0082] Step 406, determining the direction of the target in the laser radar coordinates according to the angle between the normal vector and the reference vector.
[0083] In this step, after obtaining the normal vector of the laser point cloud fitting plane and the preset reference vector, the angle between the normal vector and the reference vector can be calculated, and then the obtained angle is used as the direction of the laser point cloud fitting plane corresponding to the intercepted target three-dimensional point cloud data in the current laser radar coordinates, that is, the direction of the target in the current laser radar coordinates. Specifically, the angle can be calculated using the following formula: .
[0084] in, is the angle between the normal vector and the reference vector, v represents the normal vector, and e represents the reference vector. For example, see Fig.11 The schematic diagram of the target's direction in the laser radar coordinates is shown in That is, the direction of the target relative to the e vector in the current lidar coordinates.
[0085] In this embodiment, the direction of the target in the laser radar coordinates is determined by calculating the angle between the normal vector of the laser point cloud fitting plane and the reference vector. This process is simple and intuitive, and thus can improve the efficiency and accuracy of detecting the direction of the target in the laser radar coordinates.
[0086] In addition, it should be noted that the above Figure 3-6 , 8, and 11 are only examples, and the text, numbers, and other contents therein do not affect the essential contents of the technical solutions of the embodiments of the present invention.
[0087] The target direction detection device provided by the present invention is described below. The target direction detection device described below and the target direction detection method described above can be referred to each other.
[0088] Fig.12 is a schematic diagram of the structure of the target direction detection device provided by the present invention, see Fig.12 As shown, the device may include: The image acquisition and segmentation module 510 is used to acquire the image to be tested captured by the camera installed on the robot, and segment the target in the image to be tested to determine the target segmentation mask corresponding to the image to be tested; the target segmentation mask includes the target; The original point cloud acquisition module 520 is used to acquire the three-dimensional point cloud data collected by the laser radar installed on the robot; the three-dimensional point cloud data and the image to be measured are collected at the same time; The target point cloud acquisition module 530 is used to intercept the target three-dimensional point cloud data corresponding to the target from the three-dimensional point cloud data according to the coordinate system transformation parameters between the laser radar and the camera and the position information of the target in the target segmentation mask; The direction detection module 540 is used to perform plane fitting processing on the target three-dimensional point cloud data, obtain the laser point cloud fitting plane, and determine the direction of the target in the laser radar coordinates according to the normal vector of the laser point cloud fitting plane.
[0089] In some embodiments, before the direction detection module 540 determines the direction of the target in the laser radar coordinates according to the normal vector of the laser point cloud fitting plane, the device may further include: A judgment module is used to judge whether the target three-dimensional point cloud data is valid; an execution module, for returning to re-execute the above step of intercepting target three-dimensional point cloud data corresponding to the target from the three-dimensional point cloud data according to the coordinate system transformation parameters between the laser radar and the camera and the position information of the target in the target segmentation mask if the step is invalid, so as to obtain new target three-dimensional point cloud data; The execution module is also used to execute the above step of determining the direction of the target in the laser radar coordinates according to the normal vector of the laser point cloud fitting plane if it is valid.
[0090] Optionally, the above-mentioned judgment module is specifically used to obtain the three-dimensional coordinates of each point in the target three-dimensional point cloud data and the plane equation corresponding to the laser point cloud fitting plane; calculate the distance from each point to the laser point cloud fitting plane based on the three-dimensional coordinates and plane equation of each point; and determine whether the target three-dimensional point cloud data is valid based on each distance.
[0091] Optionally, the above-mentioned judgment module is specifically used to calculate the mean and variance corresponding to each distance; if the mean exceeds a first threshold, and / or the variance exceeds a second threshold, it is determined that the target three-dimensional point cloud data is invalid.
[0092] In some embodiments, the above-mentioned direction detection module 540 is specifically used to obtain the plane equation corresponding to the laser point cloud fitting plane, and determine the normal vector corresponding to the laser point cloud fitting plane according to the plane equation; obtain a preset reference vector; and determine the direction of the target in the laser radar coordinates according to the angle between the normal vector and the reference vector.
[0093] In some embodiments, the above-mentioned direction detection module 540 is specifically used to obtain the three-dimensional coordinates of each point in the target three-dimensional point cloud data, and calculate the three-dimensional coordinates of the center of mass corresponding to the target three-dimensional point cloud data based on the three-dimensional coordinates of each point; according to the three-dimensional coordinates of the center of mass, the three-dimensional coordinates of each point are de-centroided to determine the de-centroided three-dimensional coordinates of each point; according to the de-centroided three-dimensional coordinates of each point, a feature matrix is constructed, and the feature matrix is solved to determine the plane equation coefficients; according to the plane equation coefficients, the corresponding plane equation is constructed to determine the laser point cloud fitting plane.
[0094] In some embodiments, the target point cloud acquisition module 530 is specifically used to convert the position information of each point in the three-dimensional point cloud data from the laser radar coordinates to the image coordinate system corresponding to the target segmentation mask according to the coordinate system transformation parameters between the laser radar and the camera, and determine the position information corresponding to each point in the image coordinates; select the position information corresponding to the target in the target segmentation mask from the position information corresponding to each point in the image coordinates; according to the index corresponding to the position information of the selected target in the image coordinate system, intercept the point cloud data corresponding to the index in the three-dimensional point cloud data, and obtain the target three-dimensional point cloud data corresponding to the target.
[0095] It should be noted here that the above-mentioned device provided in the embodiment of the present invention can implement all the method steps implemented in the above-mentioned method embodiment, and can achieve the same technical effect. The parts and beneficial effects that are the same as the method embodiment in this embodiment will not be described in detail here.
[0096] Fig.13 An example of a physical structure diagram of an electronic device is shown in FIG. Fig.13 As shown, the electronic device may include: a processor 610, a communication interface 620, a memory 630 and a communication bus 640, wherein the processor 610, the communication interface 620 and the memory 630 communicate with each other through the communication bus 640. The processor 610 may call the logic instructions in the memory 630 to execute the target direction detection method, which includes: obtaining the image to be tested collected by the camera installed on the robot, and performing segmentation processing on the target in the image to be tested, and determining the target segmentation mask corresponding to the image to be tested; the above target segmentation mask includes the target; obtaining the three-dimensional point cloud data collected by the laser radar installed on the robot; the three-dimensional point cloud data is collected at the same time as the image to be tested; according to the coordinate system transformation parameters between the laser radar and the camera and the position information of the target in the target segmentation mask, intercepting the target three-dimensional point cloud data corresponding to the target from the three-dimensional point cloud data; performing plane fitting processing on the target three-dimensional point cloud data to obtain the laser point cloud fitting plane, and determining the direction of the target under the laser radar coordinates according to the normal vector of the laser point cloud fitting plane.
[0097] In addition, the logic instructions in the above-mentioned memory 630 can be implemented in the form of a software functional unit and can be stored in a computer-readable storage medium when it is sold or used as an independent product. Based on such an understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art or the part of the technical solution, can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including a number of instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk, etc. Various media that can store program codes.
[0098] On the other hand, the present invention also provides a computer program product, which includes a computer program, which can be stored on a non-transitory computer-readable storage medium. When the computer program is executed by a processor, the computer can execute the target direction detection method provided by the above-mentioned methods, and the method includes: obtaining an image to be tested captured by a camera installed on the robot, and segmenting the target in the image to be tested to determine a target segmentation mask corresponding to the image to be tested; the above-mentioned target segmentation mask includes the target; obtaining three-dimensional point cloud data captured by a laser radar installed on the robot; the above-mentioned three-dimensional point cloud data is collected at the same time as the image to be tested; according to the coordinate system transformation parameters between the laser radar and the camera and the position information of the target in the target segmentation mask, the target three-dimensional point cloud data corresponding to the target is intercepted from the three-dimensional point cloud data; plane fitting processing is performed on the target three-dimensional point cloud data to obtain a laser point cloud fitting plane, and the direction of the target in the laser radar coordinates is determined according to the normal vector of the laser point cloud fitting plane.
[0099] On the other hand, the present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, is implemented to execute the target direction detection method provided by the above-mentioned methods, the method comprising: obtaining an image to be measured captured by a camera installed on a robot, and performing segmentation processing on the target in the image to be measured, and determining a target segmentation mask corresponding to the image to be measured; the above-mentioned target segmentation mask includes the target; obtaining three-dimensional point cloud data captured by a laser radar installed on the robot; the above-mentioned three-dimensional point cloud data is acquired at the same time as the image to be measured; according to the coordinate system transformation parameters between the laser radar and the camera and the position information of the target in the target segmentation mask, intercepting the target three-dimensional point cloud data corresponding to the target from the three-dimensional point cloud data; performing plane fitting processing on the target three-dimensional point cloud data to obtain a laser point cloud fitting plane, and determining the direction of the target under the laser radar coordinates according to the normal vector of the laser point cloud fitting plane.
[0100] The device embodiments described above are merely illustrative, wherein the units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place, or they may be distributed on multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the scheme of this embodiment. Ordinary technicians in this field can understand and implement it without paying creative labor.
[0101] Through the description of the above implementation methods, those skilled in the art can clearly understand that each implementation method can be implemented by means of software plus a necessary general hardware platform, and of course, can also be implemented by hardware. Based on this understanding, the above technical solution is essentially or the part that contributes to the prior art can be embodied in the form of a software product, and the computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, a disk, an optical disk, etc., including a number of instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the methods described in each embodiment or some parts of the embodiments.
[0102] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.
Claims
1. A method for detecting the direction of a target, characterized in that: include: Acquire an image to be tested captured by a camera installed on the robot, and segment the target in the image to be tested to determine a target segmentation mask corresponding to the image to be tested; the target segmentation mask includes the target; Acquire three-dimensional point cloud data collected by the laser radar installed on the robot; the three-dimensional point cloud data and the image to be measured are collected at the same time; According to the coordinate system transformation parameters between the laser radar and the camera and the position information of the target in the target segmentation mask, intercepting the target three-dimensional point cloud data corresponding to the target from the three-dimensional point cloud data; The three-dimensional point cloud data of the target is subjected to plane fitting processing to obtain a laser point cloud fitting plane, and the direction of the target in the laser radar coordinates is determined according to the normal vector of the laser point cloud fitting plane.
2. The target direction detection method according to claim 1, characterized in that: Before determining the direction of the target in laser radar coordinates according to the normal vector of the laser point cloud fitting plane, the method further includes: Determining whether the target three-dimensional point cloud data is valid; If invalid, return to re-execute the step of intercepting target three-dimensional point cloud data corresponding to the target from the three-dimensional point cloud data according to the coordinate system transformation parameters between the laser radar and the camera and the position information of the target in the target segmentation mask to obtain new target three-dimensional point cloud data; If valid, the step of determining the direction of the target in laser radar coordinates according to the normal vector of the laser point cloud fitting plane is performed.
3. The target direction detection method according to claim 2, characterized in that: The determining whether the target three-dimensional point cloud data is valid includes: Obtaining the three-dimensional coordinates of each point in the target three-dimensional point cloud data and the plane equation corresponding to the laser point cloud fitting plane; Calculate the distance between each point and the laser point cloud fitting plane according to the three-dimensional coordinates of each point and the plane equation; Whether the target three-dimensional point cloud data is valid is determined according to each of the distances.
4. The target direction detection method according to claim 3, characterized in that: The determining whether the target three-dimensional point cloud data is valid according to each of the distances includes: Calculate the mean and variance corresponding to each of the distances; If the mean exceeds a first threshold, and / or the variance exceeds a second threshold, it is determined that the target three-dimensional point cloud data is invalid.
5. The target direction detection method according to any one of claims 1 to 4, characterized in that: The determining the direction of the target in laser radar coordinates according to the normal vector of the laser point cloud fitting plane includes: Obtaining a plane equation corresponding to the laser point cloud fitting plane, and determining a normal vector corresponding to the laser point cloud fitting plane according to the plane equation; Obtain a preset reference vector; The direction of the target in the laser radar coordinates is determined according to the angle between the normal vector and the reference vector.
6. The target direction detection method according to any one of claims 1 to 4, characterized in that: The performing plane fitting processing on the target three-dimensional point cloud data to obtain the laser point cloud fitting plane includes: Obtaining the three-dimensional coordinates of each point in the target three-dimensional point cloud data, and calculating the three-dimensional coordinates of the center of mass corresponding to the target three-dimensional point cloud data according to the three-dimensional coordinates of each point; According to the three-dimensional coordinates of the centroid, the three-dimensional coordinates of each of the points are de-centroided to determine the three-dimensional coordinates of each of the points after de-centroiding; Constructing a characteristic matrix according to the de-centroided three-dimensional coordinates of each of the points, and solving the characteristic matrix to determine the coefficients of the plane equation; A corresponding plane equation is constructed according to the plane equation coefficients to determine the laser point cloud fitting plane.
7. The target direction detection method according to any one of claims 1 to 4, characterized in that: The method of extracting target three-dimensional point cloud data corresponding to the target from the three-dimensional point cloud data according to the coordinate system transformation parameters between the laser radar and the camera and the position information of the target in the target segmentation mask comprises: According to the coordinate system transformation parameters between the laser radar and the camera, the position information of each point in the three-dimensional point cloud data is converted from the laser radar coordinates to the image coordinate system corresponding to the target segmentation mask, and the position information corresponding to each point in the image coordinates is determined; Selecting the position information corresponding to the target in the target segmentation mask from the position information corresponding to each of the points in the image coordinates; According to the index corresponding to the position information of the selected target in the image coordinate system, point cloud data corresponding to the index is intercepted in the three-dimensional point cloud data to obtain target three-dimensional point cloud data corresponding to the target.
8. A target direction detection device, characterized in that: include: An image acquisition and segmentation module, used to acquire the image to be tested captured by the camera installed on the robot, and segment the target in the image to be tested to determine the target segmentation mask corresponding to the image to be tested; the target segmentation mask includes the target; An original point cloud acquisition module, used to acquire three-dimensional point cloud data collected by the laser radar installed on the robot; the three-dimensional point cloud data and the image to be measured are collected at the same time; A target point cloud acquisition module, used to intercept target three-dimensional point cloud data corresponding to the target from the three-dimensional point cloud data according to the coordinate system transformation parameters between the laser radar and the camera and the position information of the target in the target segmentation mask; The direction detection module is used to perform plane fitting processing on the target three-dimensional point cloud data to obtain a laser point cloud fitting plane, and determine the direction of the target in the laser radar coordinates according to the normal vector of the laser point cloud fitting plane.
9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and running on the processor, characterized in that: When the processor executes the computer program, the target direction detection method according to any one of claims 1 to 7 is implemented.
10. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the method for detecting the direction of a target according to any one of claims 1 to 7 is implemented.