Method and System for Automatically Marking Traffic Lights on a Point Cloud Map

Abstract

The present invention discloses a system for automatically marking traffic lights on a point cloud map, which includes a data acquisition module, a point cloud map generation module, and a traffic light positioning module. Among them, the data acquisition module is used to collect data, the point cloud map generation module is used to generate a point cloud map based on the collected data, and the traffic light positioning module completes the marking of traffic lights on the generated point cloud map according to the collected data. The present invention mainly back-propagates the error of traffic lights detected by vision and traffic lights detected by lidar in the 3D spatial position to the extrinsic parameters between the camera and the lidar, corrects the extrinsic parameter error between the camera and the lidar, and then calibrates the traffic lights on the point cloud map. The present invention also provides a method for automatically marking traffic lights on a point cloud map. The present invention can fully realize the automatic marking of the position of traffic lights in a high-precision point cloud map, effectively improve the accuracy of calibration, greatly reduce the acquisition cost at the same time, and increase the richness of data annotation sources.

Description

Technical Field

[0001] The present invention belongs to the field of data processing, and particularly relates to a method and system for automatically marking traffic lights on a point cloud map. Background Art

[0002] With the progress of sensor technology and the rapid development of the automotive industry, advanced driver assistance technology and autonomous driving technology have made great progress in recent years. Whether it is advanced driver assistance technology or autonomous driving technology, they cannot get rid of the dependence on high-precision maps.

[0003] After years of development of navigation map drawing technology, the high-precision map drawing technology has had some automated attributes at the beginning. Simple tasks such as lane line detection and traffic sign recognition in the process of high-precision map drawing have been highly automated. However, due to various reasons, the appearances of traffic lights in each city in China are not unified. Therefore, it is only possible to rely on manual labor to mark the positions of traffic lights in high-precision point cloud maps. At the same time, there are generally more than one traffic light at intersections with multiple driving directions. At present, the industry completely relies on manual labor to determine the position of each traffic light and the corresponding driving direction. Summary of the Invention

[0004] Object of the Invention: In view of the problems existing in the prior art, the present invention proposes a system for automatically marking traffic lights on a point cloud map that fully realizes automation and effectively improves the marking accuracy.

[0005] Technical Solution: To achieve the above object, the present invention provides a system for automatically marking traffic lights on a point cloud map, including a data acquisition module, a point cloud map generation module, and a traffic light positioning module;

[0006] The data acquisition module is used to collect the environmental information and vehicle driving information of an intersection when the vehicle passes through the intersection; the environmental information includes picture information and laser point cloud information; the data acquisition module includes a camera, a lidar, and an integrated navigation unit; the camera is used to collect picture information, the lidar is used to collect laser point cloud information, and the integrated navigation unit is used to collect vehicle driving information;

[0007] The point cloud map generation module generates a high-precision point cloud map including the intersection environment based on the environmental information and vehicle driving information of the intersection collected by the data acquisition module;

[0008] The traffic light positioning module includes a position recognition sub-module for traffic lights in pictures, a position coordinate conversion sub-module for traffic lights, and an external parameter matrix correction sub-module for the lidar to the camera;

[0009] Among them, the position recognition sub-module for traffic lights in pictures identifies the positions of traffic lights with green lights on in each picture through the picture information;

[0010] The traffic light position coordinate conversion sub-module converts the position of the traffic light with a green light on to a position on the high-precision point cloud map;

[0011] The external parameter matrix correction sub-module from the lidar to the camera determines whether the external parameter matrix from the lidar to the camera needs to be corrected according to the position of the traffic light with a green light on obtained by the traffic light position coordinate conversion sub-module on the high-precision point cloud map; if no correction is required, mark the position of the traffic light with a green light on obtained currently on the high-precision point cloud map and the indicated traffic direction; if correction is required, correct the external parameter matrix from the lidar to the camera, and after correction, repeat the work of the traffic light position coordinate conversion sub-module and the external parameter matrix correction sub-module from the lidar to the camera until the external parameter matrix from the lidar to the camera does not need to be corrected.

[0012] Furthermore, the data acquisition module further includes a synchronization signal generator; the synchronization signal generator is used to synchronize the acquisition times of the camera, lidar, and integrated navigation unit. This can effectively improve the working efficiency of the system and the accuracy of positioning.

[0013] Furthermore, the lidar is a solid-state lidar; it is installed at the upper end of the vehicle's front windshield; the camera has 8 million pixels and is set inside the vehicle's front windshield.

[0014] Furthermore, the point cloud map generation module obtains a high-precision point cloud map including the intersection environment according to the information collected by the data acquisition module in combination with the point cloud registration method. The points on the obtained point cloud map are denser, and the marking of traffic lights is more accurate. At the same time, it is not necessary to use special surveying vehicles and surveying personnel, and the point cloud map can be obtained more conveniently and quickly.

[0015] The present invention also provides a method for automatically marking traffic lights on a point cloud map based on the above system for automatically marking traffic lights on a point cloud map, including the following steps:

[0016] Step 1: When the vehicle passes through an intersection, the data acquisition module starts to collect data: obtain the vehicle pose sequence Position_queue_rtk, z-axis angular velocity sequence Angle_rate_queue_rtk, point cloud sequence Pointcloud_queue_lidar, and color image sequence Image_queue_camera during the period when the vehicle passes through the intersection;

[0017] Step 2: Use the 3D pose corresponding to the acquisition time in the pose sequence Position_queue_rtk as the initial position, and combine the point cloud registration method to sequentially splice adjacent point cloud frames in the point cloud sequence Pointcloud_queue_lidar, obtaining a high-precision point cloud map Pointcloud_map_lidar of the intersection environment containing traffic lights and a sequence of pose change matrices Matrix_odometry_lidar of adjacent point cloud frames sorted by acquisition time;

[0018] Step 3: Run the traffic light recognition algorithm on each color picture in the color picture sequence Image_queue_camera in sequence, and form a traffic light position sequence BoundingBox_camera with the positions of all recognized traffic lights;

[0019] Step 4: Screen out the positions of the traffic lights with green lights on in the traffic light position sequence BoundingBox_camera, obtaining a traffic light position sequence BoundingBox_camera_green of the traffic lights with green lights on;

[0020] Step 5: Convert the position coordinate values of each traffic light in the traffic light position sequence BoundingBox_camera_green of the traffic lights with green lights on obtained in Step 4 from the picture pixel coordinate system to the coordinate values in the high-precision point cloud map Pointcloud_map_lidar, obtaining a traffic light contour box position sequence BoundingBox_lidar_frame of the traffic lights with green lights on in the high-precision point cloud map Pointcloud_map_lidar;

[0021] Step 6: Sequentially calculate the seed point coordinates of each traffic light with a green light on according to the traffic light contour box position sequence BoundingBox_lidar_frame in the high-precision point cloud map Pointcloud_map_lidar obtained in Step 5;

[0022] Step 7: Perform clustering with each seed point obtained in Step 6 respectively, sequentially obtaining the clustered point clouds of each traffic light with a green light on; and obtaining the centroid corresponding to each clustered point cloud;

[0023] Step 8: Calculate the distances between the centroid of each clustered point cloud and the center lines of each traffic light bounding box in the traffic light bounding box position sequence BoundingBox_lidar_frame, and save the shortest distance among the distances from the centroid of each clustered point cloud to the center lines of each traffic light bounding box in the traffic light bounding box position sequence BoundingBox_lidar_frame into the shortest distance set Point_to_line_distance_set;

[0024] Step 9: Calculate the sum of the shortest distances according to the formula and compare the calculated sum of the shortest distances with the set threshold. If it is less than the threshold, execute Step 10; if it is not less than the threshold, execute Steps 11 to 12; where M represents the total number of traffic light bounding boxes in the traffic light bounding box position sequence BoundingBox_lidar_frame; i represents the number; Point_to_line_distance_set[i] represents the shortest distance from the centroid of the clustered point cloud of the i-th traffic light with a green light on in the shortest distance set Point_to_line_distance_set to the position of the traffic light bounding box;

[0025] Step 10: Mark the traffic light bounding box position sequence BoundingBox_lidar_frame in the high-precision point cloud map Pointcloud_map_lidar obtained in Step 5 on the point cloud map Pointcloud_map_lidar, and mark the status of the traffic lights;

[0026] Step 11: Use the Gauss-Newton method to solve the error matrix of the extrinsic matrix from the radar to the camera such that

[0027] Step 12: Correct the extrinsic matrix from the lidar to the camera according to the formula ; According to the corrected extrinsic matrix from the lidar to the camera, repeat Steps 5 to 9; is the extrinsic matrix from the lidar to the camera.

[0028] Furthermore, the clustering method in Step 7 includes the following steps:

[0029] Step 701: Calculate the average value of the z-axis coordinate values of all points on the i-th traffic light bounding box BoundingBox_lidar_frame in the high-precision point cloud map Pointcloud_map_lidar to obtain Height_boundingBox_lidar_frame i ​i ;

[0030] Step 702: Traverse all the points in the i-th clustering range Pointcloud_map_lidar_cropped_i. When a point Point_j satisfies the following conditions simultaneously with all other points in the i-th clustering range Pointcloud_map_lidar_cropped_i, retain point Point_j into the clustered point cloud cluster_set_i:

[0031] |Ponit j .x - Point n .x| < 0.1;

[0032] |Ponit j .y - Point n .y| < 0.1;

[0033] |Ponit j .z - Point n .z| > 0.5 * Height_boundingBox_lidar_frame i ;

[0034] Among them, the setting method of the i-th clustering range Pointcloud_map_lidar_cropped_i is: taking the seed point coordinate seed i as the center of the cube, with the semi-axis length of the x-axis being 5 meters, the semi-axis length of the y-axis being 5 meters, and the semi-axis length of the z-axis being 1.5 meters; cluster_set_i represents the clustered point cloud of the i-th traffic light with a green light on, Ponit j .x, Ponit j .y, and Ponit j .z respectively represent the x-axis, y-axis, and z-axis coordinate values of the j-th point in the i-th clustering range Pointcloud_map_lidar_cropped_i, where j = 1, 2,..., N; N represents the total number of points in the i-th clustering range Pointcloud_map_lidar_cropped_i; Point n .x, Point n .y, and Point n .z respectively represent the x-axis, y-axis, and z-axis coordinate values of the n-th point in the i-th clustering range Pointcloud_map_lidar_cropped_i, where n = 1, 2,..., N and n ≠ j. The present invention completes clustering by searching for adjacent points within the cube Euclidean space search range, making the clustering result more accurate.

[0035] Further, the status of the traffic lights marked in step 10 includes the direction indicated by the traffic lights. The method for identifying the direction indicated by the traffic lights is as follows: traverse the values of the combined navigation z-axis angular velocity sequence Angle_rate_queue_rtk corresponding to the time period when the vehicle passes through the intersection, and obtain the status of the green light Direction_traffic_light when the vehicle passes through the intersection;

[0036] 1) If at least one of all the values in the z-axis angular velocity sequence Angle_rate_queue_rtk is greater than 0.2, it indicates that the current traffic light status is a left-turn green light;

[0037] 2) If at least one of all the values in the z-axis angular velocity sequence Angle_rate_queue_rtk is less than -0.2, it indicates that the current traffic light status is a right-turn green light;

[0038] 3) If none of the values in the z-axis angular velocity sequence Angle_rate_queue_rtk is greater than 0.2 and less than -0.2, it indicates that the current traffic light status is a straight-ahead green light. Using such a method to identify the indication method of traffic lights is faster and more accurate.

[0039] The present invention also provides a computer system, including:

[0040] One or more processors;

[0041] A memory storing operable instructions, and the instructions, when executed by the one or more processors, cause the one or more processors to perform operations, and the operations include the process of the method for automatically marking traffic lights on the point cloud map as described above.

[0042] The present invention also provides a computer-readable medium storing software, and the software includes instructions executable by one or more computers. The instructions, when executed in this way, cause the one or more computers to perform operations, and the operations include the process of the method for automatically marking traffic lights on the point cloud map as described above.

[0043] Beneficial effects: Compared with the prior art, the method provided by the present invention can fully realize the automatic annotation of the positions of traffic lights in a high-precision point cloud map; the present invention back-propagates the error of the traffic lights detected by vision and the traffic lights detected by lidar in the 3D spatial position to the external parameters between the camera and the lidar, overcomes the error in the external parameter calibration when the camera and the lidar leave the factory, and effectively improves the accuracy of calibration; at the same time, the present invention can be based on the data collected by any ordinary vehicle equipped with advanced assisted driving data collection devices (including high-definition cameras, etc.), greatly reducing the collection cost, and the data collected by each vehicle can be used to annotate the positions of traffic lights, increasing the richness of the data annotation sources. Description of the Drawings

[0044] Figure 1 It is a schematic diagram of the system provided by the present invention;

[0045] Figure 2 It is a schematic diagram of the method flow provided by the present invention;

[0046] Figure 3 It is a schematic diagram of a right-handed coordinate system;

[0047] Figure 4 It is a schematic diagram of identifying traffic lights in a picture provided by the present invention;

[0048] Figure 5 It is a schematic diagram of screening out traffic lights with green lights on in a picture provided by the present invention;

[0049] Figure 6 It is a schematic diagram of the clustering range provided by the present invention. Detailed Embodiments

[0050] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

[0051] As Figure 1 shown, this embodiment provides a system for automatically annotating traffic lights on a point cloud map, which is applied to mass-produced cars with autonomous driving functions, and mainly includes a data collection module, a point cloud map generation module, and a traffic light positioning module.

[0052] Among them, the data acquisition module includes a solid-state lidar, a camera, an integrated navigation unit, and a synchronization signal generator; the solid-state lidar is installed at the upper end of the vehicle's front windshield; the camera has 8 million pixels and is set inside the vehicle's front windshield; both the synchronization signal generator and the integrated navigation are set inside the vehicle. Denote the extrinsic parameter matrix from the camera to the lidar calibrated at the vehicle factory as Denote the extrinsic parameter matrix from the lidar to the camera calibrated at the vehicle factory as The data acquisition module is used to collect the environmental information and vehicle driving information of the intersection when the vehicle passes through the intersection.

[0053] The integrated navigation unit sends a 1 Hz PPS satellite synchronization signal to the synchronization signal generator, and the synchronization signal generator sends a 20 Hz sensor data acquisition synchronization signal according to the 1 Hz PPS signal sent by the integrated navigation. After receiving the 20 Hz synchronization signal, the lidar starts to emit laser and collect the point cloud of the surrounding environment every time the synchronization signal arrives. After receiving the 20 Hz synchronization signal, the camera collects the color picture signal of the surrounding environment every time the synchronization signal arrives. When the vehicle passes through the intersection, the data acquisition module starts the data acquisition work.

[0054] The point cloud map generation module obtains a high-precision point cloud map including the intersection environment through the point cloud registration method based on the point cloud sequence and vehicle pose sequence collected by the data acquisition module.

[0055] The traffic light positioning module includes a traffic light position recognition sub-module in the picture, a traffic light position coordinate conversion sub-module, and an extrinsic parameter matrix correction sub-module from the lidar to the camera;

[0056] Among them, the traffic light position recognition sub-module in the picture identifies the position of the traffic light with a green light on in each picture through the picture information;

[0057] The traffic light position coordinate conversion sub-module converts the position of the traffic light with a green light on to the position on the high-precision point cloud map;

[0058] The extrinsic parameter matrix correction sub-module from the lidar to the camera determines whether the extrinsic parameter matrix from the lidar to the camera needs to be corrected according to the position of the traffic light with a green light on obtained by the traffic light position coordinate conversion sub-module on the high-precision point cloud map; if no correction is needed, mark the position of the traffic light with a green light on obtained currently on the high-precision point cloud map and the indicated traffic direction; if correction is needed, correct the extrinsic parameter matrix from the lidar to the camera, and after correction, repeat the work of the traffic light position coordinate conversion sub-module and the extrinsic parameter matrix correction sub-module from the lidar to the camera until the extrinsic parameter matrix from the lidar to the camera does not need to be corrected.

[0059] When a vehicle equipped with the system for automatically marking traffic lights on the point cloud map provided in this embodiment passes through an intersection containing traffic lights, the method for automatically marking traffic lights on the point cloud map disclosed in this embodiment is triggered. As Figure 2 shown, it specifically includes the following steps:

[0060] Step 1: The data acquisition module starts to acquire data:

[0061] The integrated navigation continuously acquires the 3D pose and z-axis angular velocity of the vehicle during the time when the vehicle passes through the intersection, and forms a pose sequence Position_queue_rtk of the vehicle passing through the intersection and a z-axis angular velocity sequence Angle_rate_queue_rtk according to the data acquired at each acquisition moment; among them, the pose sequence includes the coordinate values of the vehicle in the geodetic coordinate system at each acquisition moment. The integrated navigation output data is based on the right-hand coordinate system. As Figure 3 shown, the z-axis angular velocity is the angular velocity of rotation around the z-axis.

[0062] The solid-state lidar continuously acquires the point cloud of the vehicle's surrounding environment during the time when the vehicle passes through the intersection, and forms a point cloud sequence Pointcloud_queue_lidar according to the point cloud frames acquired at each acquisition moment.

[0063] The camera continuously acquires the color pictures of the vehicle's surrounding environment during the time when the vehicle passes through the intersection, and forms a color picture sequence Image_queue_camera according to the color pictures acquired at each acquisition moment.

[0064] Since a synchronization signal generator is set in the data acquisition module, the acquisition moments of the integrated navigation, solid-state lidar, and camera for acquiring data are the same. Therefore, within the same data acquisition cycle, the total amounts of data in the pose sequence Position_queue_rtk, z-axis angular velocity sequence Angle_rate_queue_rtk, point cloud sequence Pointcloud_queue_lidar, and color picture sequence Image_queue_camera are the same.

[0065] Step 2: The point cloud map generation module uses the 3D pose corresponding to the acquisition moment in the pose sequence Position_queue_rtk as the initial position, and combines the point cloud registration method to splice adjacent point cloud frames in the point cloud sequence Pointcloud_queue_lidar in turn, to obtain a high-precision point cloud map Pointcloud_map_lidar of the intersection environment containing traffic lights and a pose change matrix sequence Matrix_odometry_lidar of adjacent point cloud frames sorted according to the acquisition moment. The point cloud registration method includes the ICP point cloud registration method or the NDT point cloud registration method.

[0066] Step 3: Run the traffic light recognition algorithm on each color picture in the color picture sequence Image_queue_camera in turn. As Figure 4 shown, identify the traffic lights in each color picture. Generally, the contour box BoundingBox of the identified traffic lights is a quadrilateral. Therefore, store the coordinate values of the four corner points of the contour box BoundingBox of each identified traffic light as the position of the corresponding traffic light, and form a traffic light position sequence BoundingBox_camera with the positions of all identified traffic lights.

[0067] The coordinate values stored in the traffic light position sequence BoundingBox_camera are in the picture pixel coordinate system. Among them, traditional CV template matching method or model-based inference method can be used to identify traffic lights in color pictures.

[0068] Step 4: Combine and filter each color picture in the color picture sequence Image_queue_camera in its corresponding traffic light position sequence BoundingBox_camera. As Figure 5 shown, retain the positions of the traffic lights that are green during the period when the vehicle passes through the intersection, and obtain a traffic light position sequence BoundingBox_camera_green of the traffic lights that are green.

[0069] Step 5: Convert the position coordinate values of each traffic light in the traffic light position sequence BoundingBox_camera_green of the traffic lights that are green obtained in Step 4 from the picture pixel coordinate system to the coordinate values in the high-precision point cloud map Pointcloud_map_lidar, and obtain a traffic light contour box position sequence BoundingBox_lidar_frame of the traffic lights that are green in the high-precision point cloud map Pointcloud_map_lidar. The high-precision point cloud map Pointcloud_map_lidar is in the earth coordinate system.

[0070] Specifically, the method for coordinate value conversion includes the following steps:

[0071] Step 501: Starting from the color picture with the earliest acquisition time in the color picture sequence Image_queue_camera, take two adjacent color pictures in terms of acquisition time in turn and record them as Image a and Image a+1 . In the pose change matrix sequence Matrix_odometry_lidar, take the ones corresponding to Image a and Image a+1Pose change matrix Matrix_odometry_lidar corresponding to lidar point clouds with the same acquisition time for two frames of color images a . Among them, Image a represents the color image acquired by the camera at the a-th acquisition time, and Image a+1 represents the color image acquired by the camera at the (a + 1)-th acquisition time. Matrix_odometry_lidar a represents the pose change matrix between the point cloud frame acquired by the solid-state lidar at the a-th acquisition time and the point cloud frame acquired at the (a + 1)-th acquisition time.

[0072] Step 502: According to the formula:

[0073]

[0074] Calculate the pose change matrix Matrix_odometry_camera from the pose of the camera when acquiring the color image Image a at the a-th acquisition time to the pose of the camera when acquiring the color image Image a at the (a + 1)-th acquisition time. a .

[0075] Step 503: According to the pose change matrix Matrix_odometry_camera a obtained in Step 502 and the position sequence BoundingBox_camera_green of the traffic light with a green light obtained in Step 4, and combined with the camera internal parameters, obtain the 3D position BoundingBox_camera_frame a of the traffic light with a green light BoundingBox_camera_green a in the Image a camera coordinate system. BoundingBox_camera_green a represents the position of the traffic light with a green light in the color image acquired by the camera at the a-th acquisition time; BoundingBox_camera_frame a represents the 3D position obtained by converting the position of the traffic light with a green light in the color image acquired by the camera at the a-th acquisition time to the Image a camera coordinate system.

[0076] Step 504: According to the formula:

[0077]

[0078] Calculate the BoundingBox_camera_frame a The 3D position BoundingBox_lidar_frame in the point cloud map coordinate system a ;

[0079] Among them, represents the pose change matrix between the point cloud frame collected at the first acquisition time and the point cloud frame collected at the a-th acquisition time in the point cloud sequence Pointcloud_queue_lidar composed of point cloud frames;

[0080]

[0081] is the pose change matrix between the point cloud frame collected at the first acquisition time and the point cloud frame collected at the second acquisition time; is the pose change matrix between the point cloud frame collected at the second acquisition time and the point cloud frame collected at the third acquisition time; is the pose change matrix between the point cloud frame collected at the a-th acquisition time and the point cloud frame collected at the (a - 1)-th acquisition time; All are elements in the pose change matrix sequence Matrix_odometry_lidar.

[0082] Step 6: According to the sequence of traffic light contour box positions BoundingBox_lidar_frame in the high-precision point cloud map Pointcloud_map_lidar obtained in Step 5, calculate the seed point coordinates of each traffic light with a green light on in turn. Calculate the seed point coordinates seed of the i-th traffic light with a green light on according to the following formula i (x seedi , y seedi , z seedi ):

[0083]

[0084] Among them, x seedi , y seedi and z seedi respectively represent the coordinate values of the x-axis, y-axis, and z-axis of the seed point of the i-th traffic light with a green light on in the earth coordinate system; x i1 , y i1 and z i1 respectively represent the coordinate values of the x-axis, y-axis, and z-axis of the first corner point of the contour box of the i-th traffic light with a green light on in the earth coordinate system; x i2 , y i2 and z i2respectively represent the x-axis, y-axis, and z-axis coordinate values of the second corner point of the contour box of the i-th traffic light with a green light on in the geodetic coordinate system; x i3 , y i3 and z i3 respectively represent the x-axis, y-axis, and z-axis coordinate values of the third corner point of the contour box of the i-th traffic light with a green light on in the geodetic coordinate system; x i4 , y i4 and z i4 respectively represent the x-axis, y-axis, and z-axis coordinate values of the fourth corner point of the contour box of the i-th traffic light with a green light on in the geodetic coordinate system.

[0085] Step 7: Set a corresponding clustering range for each seed point obtained in Step 6 in the high-precision point cloud map Pointcloud_map_lidar obtained in Step 2; perform clustering operations within the set clustering ranges to successively obtain the clustered point clouds of each traffic light with a green light on; and obtain the centroid corresponding to each clustered point cloud.

[0086] Among them, the method for setting the clustering range is: as Figure 6 shown, with the seed point coordinate seed i as the center of the cube, the cube is 10 meters long along the x-axis direction or parallel to the x-axis direction, 10 meters long along the y-axis direction or parallel to the y-axis direction, and 3 meters long along the z-axis direction or parallel to the z-axis direction.

[0087] The clustering operation mainly includes the following steps:

[0088] Step 701: Calculate the average value of the z-axis coordinate values of the four corner points on the traffic light contour box BoundingBox_lidar_frame i in the high-precision point cloud map Pointcloud_map_lidar for the i-th one to obtain Height_boundingBox_lidar_frame i ; that is

[0089] Step 702: Traverse all the points in the i-th clustering range Pointcloud_map_lidar_cropped_i. When a point Point_j satisfies the following conditions simultaneously with all other points in the i-th clustering range Pointcloud_map_lidar_cropped_i, retain the point Point_j to the clustered point cloud cluster_set_i:

[0090] |Ponit j .x - Point n .x| < 0.1;

[0091] |Point j .y - Point n .y < 0.1;

[0092] |Point j .z - Point n .z > 0.5 * Height_boundingBox_lidar_frame i ;

[0093] Among them, cluster_set_i represents the clustered point cloud of the i-th traffic light with a green light on. Point j .x, Point j .y, and Point j .z respectively represent the x-axis, y-axis, and z-axis coordinate values of the j-th point in the i-th clustered range Pointcloud_map_lidar_cropped_i, where j = 1, 2,..., N; N represents the total number of points in the i-th clustered range Pointcloud_map_lidar_cropped_i; Point n .x, Point n .y, and Point n .z respectively represent the x-axis, y-axis, and z-axis coordinate values of the n-th point in the i-th clustered range Pointcloud_map_lidar_cropped_i, where n = 1, 2,..., N and n ≠ j.

[0094] Step 8: Calculate the distances from the centroid of each clustered point cloud to the center lines of each traffic light bounding box in the traffic light bounding box position sequence BoundingBox_lidar_frame, and save the shortest distance among the distances from the centroid of each clustered point cloud to the center lines of each traffic light bounding box in the traffic light bounding box position sequence BoundingBox_lidar_frame to the shortest distance set Point_to_line_distance_set.

[0095] Among them, the calculation method for the center line of each traffic light bounding box in the traffic light bounding box position sequence BoundingBox_lidar_frame is:

[0096] According to the formula:

[0097]

[0098]

[0099] The coordinate values of points E and F are calculated, and the straight line passing through points E and F is the center line of the current traffic light bounding box. Among them, the z-axis coordinates of the first corner point and the second corner point of the traffic light bounding box are both greater than the z-axis coordinates of the third corner point and the fourth corner point; E i .x, E i .y and E i .z respectively represent the coordinate values of point E on the bounding box of the i-th traffic light with a green light on; F i .x, F i .y and F i .z respectively represent the coordinate values of point F on the bounding box of the i-th traffic light with a green light on.

[0100] Step 9: According to the formula Calculate the sum of the shortest distances, and compare the calculated sum of the shortest distances with the set threshold. If it is less than the threshold, execute Step 10; if it is not less than the threshold, execute Steps 11 to 12. Among them, M represents the total number of traffic light bounding boxes in the traffic light bounding box position sequence BoundingBox_lidar_frame; i represents the number; Point_to_line_distance_set[i] represents the shortest distance from the centroid of the clustered point cloud of the i-th traffic light with a green light on in the shortest distance set Point_to_line_distance_set to the traffic light bounding box position.

[0101] Step 10: Mark the traffic light bounding box position sequence BoundingBox_lidar_frame in the high-precision point cloud map Pointcloud_map_lidar obtained in Step 5 on the point cloud map Pointcloud_map_lidar, and mark the status of the traffic light; the status of the traffic light includes the direction indicated by the traffic light.

[0102] Among them, the recognition method of the direction indicated by the traffic light is: traverse the values of the z-axis angular velocity sequence Angle_rate_queue_rtk corresponding to the time period when the vehicle passes through the intersection, and obtain the status of the green light Direction_traffic_light when the vehicle passes through the intersection.

[0103] 1) If at least one of all the values in the z-axis angular velocity sequence Angle_rate_queue_rtk is greater than 0.2, it means that the current traffic light status is a left-turn green light.

[0104] 2) If at least one of all the values in the z-axis angular velocity sequence Angle_rate_queue_rtk is less than -0.2, it means that the current traffic light status is a right-turn green light.

[0105] 3) If none of the values in the z-axis angular velocity sequence Angle_rate_queue_rtk is greater than 0.2 and less than -0.2, it indicates that the current traffic light status is a straight-ahead green light.

[0106] Step 11: Use the Gauss-Newton method to solve the error matrix of the extrinsic matrix from the radar to the camera such that

[0107] Step 12: According to the formula Correct the extrinsic matrix from the lidar to the camera; according to the corrected extrinsic matrix from the lidar to the camera, repeat Steps 5 to 9.

[0108] The present invention also provides a computer system, including: one or more processors; a memory storing operable instructions, and when the instructions are executed by the one or more processors, the one or more processors perform operations, and the operations include the process of the method for automatically marking traffic lights on a point cloud map as described above.

[0109] It should be understood that the examples of the method for automatically marking traffic lights on a point cloud map of the present invention can be in any computer system including data storage and data processing. The aforementioned computer system can be at least one electronic processing system or electronic device including a processor and a memory, such as a PC computer, whether it is a personal PC computer, a commercial PC computer, or a PC computer for graphics processing, a server-level PC computer. These PC computers achieve wired and / or wireless data transmission, especially image data, through a data interface and / or a network interface.

[0110] In some other embodiments, the computer system can also be a server, especially a cloud server, having data storage, processing, and network communication functions.

[0111] A computer system as an example generally includes at least one processor, a memory, and a network interface connected by a system bus. The network interface is used for communicating with other devices / systems.

[0112] The processor is used to provide the computing and control of the system.

[0113] The memory includes non-volatile memory and a cache.

[0114] The non-volatile memory usually has a large storage capacity and can store an operating system and computer programs. These computer programs can include operable instructions, and when these instructions are executed by one or more processors, the one or more processors can execute the process of the method for automatically marking traffic lights on a point cloud map in the foregoing embodiments of the present invention.

[0115] In a required or reasonable implementation, the aforementioned computer system, whether a PC device or a server, may also include more or fewer components, or combinations, than those shown in the figure, or use different hardware, software, or other different components or different deployment methods.

Claims

1. A system for automatically marking traffic lights on a point cloud map, characterized in that: it includes a data acquisition module, a point cloud map generation module, and a traffic light positioning module; The data acquisition module is used to collect the environmental information and vehicle driving information of the intersection when the vehicle passes through the intersection; the environmental information includes picture information and lidar point cloud information; the data acquisition module includes a camera, a lidar, and an integrated navigation unit; the camera is used to collect picture information, the lidar is used to collect lidar point cloud information, and the integrated navigation unit is used to collect vehicle driving information; The point cloud map generation module generates a high-precision point cloud map including the intersection environment according to the environmental information and vehicle driving information of the intersection collected by the data acquisition module; The traffic light positioning module includes a position recognition sub-module of the traffic light in the picture, a traffic light position coordinate conversion sub-module, and an external parameter matrix correction sub-module from the lidar to the camera; Among them, the position recognition sub-module of the traffic light in the picture identifies the position of the traffic light with a green light on in each picture through the picture information; The traffic light position coordinate conversion sub-module converts the position of the traffic light with a green light on to the position on the high-precision point cloud map; The external parameter matrix correction sub-module from the lidar to the camera determines whether it is necessary to correct the external parameter matrix from the lidar to the camera according to the position of the traffic light with a green light on obtained by the traffic light position coordinate conversion sub-module on the high-precision point cloud map; If no correction is required, mark the position of the traffic light with a green light on obtained currently on the high-precision point cloud map and the indicated traffic direction; if correction is required, correct the external parameter matrix from the lidar to the camera, and repeat the work of the traffic light position coordinate conversion sub-module and the external parameter matrix correction sub-module from the lidar to the camera after correction until it is not necessary to correct the external parameter matrix from the lidar to the camera.

2. The system for automatically marking traffic lights on a point cloud map according to claim 1, characterized in that: The data acquisition module further includes a synchronization signal generator; the synchronization signal generator is used to synchronize the acquisition times of the camera, the lidar, and the integrated navigation unit.

3. The system for automatically marking traffic lights on a point cloud map according to claim 1, characterized in that: The lidar is a solid-state lidar; it is installed at the upper end of the vehicle's front windshield; the camera has 8 million pixels and is set inside the vehicle's front windshield.

4. The system for automatically marking traffic lights on a point cloud map according to claim 1, characterized in that: The point cloud map generation module generates a high-precision point cloud map including the intersection environment according to the information collected by the data acquisition module in combination with the point cloud registration method.

5. A method for automatically marking traffic lights on a point cloud map based on the system for automatically marking traffic lights on a point cloud map according to claim 1, characterized in that: It includes the following steps: Step 1: When the vehicle passes through the intersection, the data acquisition module starts to collect data: obtain the vehicle pose sequence Position_queue_rtk, z-axis angular velocity sequence Angle_rate_queue_rtk, point cloud sequence Pointcloud_queue_lidar, and color image sequence Image_queue_camera during the period when the vehicle passes through the intersection; Step 2: Use the 3D pose corresponding to the acquisition moment in the pose sequence Position_queue_rtk as the initial position, and combine the point cloud registration method to splice adjacent point cloud frames in the point cloud sequence Pointcloud_queue_lidar in turn, to obtain the high-precision point cloud map Pointcloud_map_lidar of the intersection environment containing traffic lights and the pose change matrix sequence Matrix_odometry_lidar of adjacent point cloud frames sorted by acquisition time; Step 3: Run the traffic light recognition algorithm on each color image in the color image sequence Image_queue_camera in turn, and form the position sequence of all recognized traffic lights into the traffic light position sequence BoundingBox_camera; Step 4: Screen out the positions of the traffic lights with green lights on in the traffic light position sequence BoundingBox_camera, and obtain the position sequence BoundingBox_camera_green of the traffic lights with green lights on; Step 5: Convert the position coordinate values of each traffic light in the position sequence BoundingBox_camera_green of the traffic lights with green lights on obtained in Step 4 from the image pixel coordinate system to the coordinate values in the high-precision point cloud map Pointcloud_map_lidar, and obtain the position sequence BoundingBox_lidar_frame of the traffic light contour boxes with green lights on in the high-precision point cloud map Pointcloud_map_lidar; Step 6: Calculate the seed point coordinates of each traffic light with a green light on in turn according to the traffic light contour box position sequence BoundingBox_lidar_frame in the high-precision point cloud map Pointcloud_map_lidar obtained in Step 5; Step 7: Cluster with each seed point obtained in Step 6 respectively to obtain the clustered point clouds of each traffic light with a green light on in turn; and obtain the centroid corresponding to each clustered point cloud; Step 8: Calculate the distances between the centroid of each clustered point cloud and the center lines of each traffic light bounding box in the traffic light bounding box position sequence BoundingBox_lidar_frame, and save the shortest distance among the distances from the centroid of each clustered point cloud to the center lines of each traffic light bounding box in the traffic light bounding box position sequence BoundingBox_lidar_frame into the shortest distance set Point_to_line_distance_set; Step 9: According to the formula calculate the sum of the shortest distances, and compare the calculated sum of the shortest distances with the set threshold. If it is less than the threshold, execute Step 10; if it is not less than the threshold, execute Steps 11 to 12; where M represents the total number of traffic light bounding boxes in the traffic light bounding box position sequence BoundingBox_lidar_frame; i represents the number; Point_to_line_distance_set[i] represents the shortest distance from the centroid of the clustered point cloud of the i-th traffic light with a green light on in the shortest distance set Point_to_line_distance_set to the position of the traffic light bounding box. Step 10: Mark the traffic light bounding box position sequence BoundingBox_lidar_frame obtained in Step 5 on the high-precision point cloud map Pointcloud_map_lidar, and mark the status of the traffic lights; Step 11: Use the Gauss-Newton method to solve the error matrix of the extrinsic matrix from the radar to the camera such that Step 12: According to the formula correct the extrinsic matrix from the lidar to the camera; according to the corrected extrinsic matrix from the lidar to the camera, repeat steps 5 to 9; is the extrinsic matrix from the lidar to the camera.

6. The method for automatically marking traffic lights on a point cloud map according to claim 5, characterized in that: The clustering method in the said Step 7 includes the following steps: Step 701: Calculate the average value of the z-axis coordinate values of all points on the traffic light bounding box BoundingBox_lidar_frame of the i-th point in the high-precision point cloud map Pointcloud_map_lidar to obtain Height_boundingBox_lidar_frame i ; i ; Step 702: Traverse all the points in the i-th clustering range Pointcloud_map_lidar_cropped_i. When a point Point_j satisfies the following conditions simultaneously with all other points in the i-th clustering range Pointcloud_map_lidar_cropped_i, retain the point Point_j into the clustered point cloud cluster_set_i: |Point j .x - Point n .x| < 0.1; |Point j .y - Point n .y | < 0.1; |Point j .z - Point n .z| > 0.5 * Height_boundingBox_lidar_frame i ; Among them, the setting method of the i-th clustering range Pointcloud_map_lidar_cropped_i is as follows: taking the seed point coordinates seed i as the center of the cube, the semi-axis length of the x-axis is 5 meters, the semi-axis length of the y-axis is 5 meters, and the semi-axis length of the z-axis is 1.5 meters; cluster_set_i represents the clustered point cloud of the i-th traffic light with a green light on, Ponit j .x, Ponit j .y and Ponit j .z respectively represent the x-axis, y-axis, and z-axis coordinate values of the j-th point in the i-th clustering range Pointcloud_map_lidar_cropped_i, where j = 1, 2,..., N; N represents the total number of points in the i-th clustering range Pointcloud_map_lidar_cropped_i; Point n .x, Point n .y and Point n .z respectively represent the x-axis, y-axis, and z-axis coordinate values of the n-th point in the i-th clustering range Pointcloud_map_lidar_cropped_i, where n = 1, 2,..., N and n ≠ j.

7. The method for automatically marking traffic lights on a point cloud map according to claim 5, characterized in that: The status marking of the traffic lights in the said Step 10 includes the direction indicated by the traffic lights. The recognition method of the direction indicated by the traffic lights is: Traverse the values of the z-axis angular velocity sequence Angle_rate_queue_rtk corresponding to the time period when the vehicle passes through the intersection, and obtain the status of the green light Direction_traffic_light when the vehicle passes through the intersection; 1) If at least one of all the values in the z-axis angular velocity sequence Angle_rate_queue_rtk is greater than 0.2, it indicates that the current traffic light status is a left-turn green light; 2) If at least one of all the values in the z-axis angular velocity sequence Angle_rate_queue_rtk is less than -0.2, it indicates that the current traffic light status is a right-turn green light; 3) If none of the values in the z-axis angular velocity sequence Angle_rate_queue_rtk is greater than 0.2 and less than -0.2, it indicates that the current traffic light status is a straight-ahead green light.

8. A computer system, characterized in that, comprising: one or more processors; a memory storing operable instructions, which when executed by the one or more processors cause the one or more processors to perform operations, the operations including the process of the method for automatically marking traffic lights on a point cloud map as described in any one of claims 5 - 7.

9. A computer-readable medium storing software, characterized in that, The software includes instructions executable by one or more computers, and such execution of the instructions causes the one or more computers to perform operations, the operations including the process of the method for automatically marking traffic lights on a point cloud map as described in any one of claims 5-7.

Images