Method and system for determining spatial transformation using local dimension iterative closest point determination

Abstract

The present invention relates to methods and systems employing local dimension iterative closest point determination for spatial transformation. A spatial surveillance system employs local dimension iterative closest point analysis to provide improved accuracy of point cloud registration. The local dimension iterative closest point analysis improves registration accuracy by performing optimization according to the size of the error in each dimension, with dimensions having large initial errors being significantly improved, and dimensions having high initial accuracy being further improved. Registration optimizes each dimension individually using surfaces with contribution information for optimizing the dimension.

Description

[0001] introduction. Background Technology

[0002] Space surveillance systems employ space sensors to monitor the surrounding environment in order to determine the movement of elements in the surrounding environment, the movement of the device to which the space surveillance system is applied, and the movement of elements in the surrounding environment relative to the movement of the device to which the space surveillance system is applied.

[0003] Vehicles, including non-autonomous, semi-autonomous, and autonomous vehicles, can use spatial monitoring systems with spatial sensors to monitor their surroundings for purposes such as trajectory planning, route selection, and collision avoidance.

[0004] There is a need for systems, methods, and devices that can process information from space surveillance systems more efficiently to dynamically provide motion-sensing information, which can be used to enhance autonomous driving and other systems. Summary of the Invention

[0005] The concepts described in this paper provide a method, system, and / or device for space surveillance systems that employ local dimensional iterative nearest-neighbor analysis, which provides improved accuracy for point cloud registration. Local dimensional iterative nearest-neighbor analysis improves registration accuracy by performing optimization based on the error magnitude of each dimension, where dimensions with large initial errors are significantly improved, and dimensions with high initial accuracy are further improved. Registration uses surfaces with contribution information for optimizing each dimension to optimize it separately.

[0006] One aspect of this disclosure includes a spatial sensor and a controller, wherein the spatial sensor is arranged to capture multiple consecutive point clouds of a field of view, and the controller performs point cloud registration on the multiple consecutive point clouds. The controller communicates with the spatial sensor and has an instruction set executable to identify multiple source points associated with a first point cloud and multiple target points associated with a second point cloud via the spatial sensor. Normal vector calculation is performed for each of the multiple target points, and each of the multiple target points is classified into one of the x, y, or z dimensions based on the normal vector calculation. An initial transformation between the multiple source points and the multiple target points is determined. For each of the x, y, and z dimensions, the association between the source points and the target points of the multiple source points is determined. The correlation cost between the multiple source points and the multiple target points is determined via root mean square (RMS) calculation. A new transformation is determined based on the x, y, and z dimensions and the correlation cost between the multiple source points and the multiple target points. The source points in the x, y, and z dimensions are updated based on the new transformation, and a spatial transformation between the multiple source points and the multiple target points is determined based on the updated source points in the x, y, and z dimensions.

[0007] Another aspect of this disclosure includes performing normal vector calculation for each of a plurality of target points by calculating normal vectors for surrounding target points in the target point, estimating the surface plane of the target point, and estimating the normal vectors of the surface plane.

[0008] Another aspect of this disclosure includes classifying multiple target points into one of the x, y, or z dimensions based on the normal vector by classifying each of the multiple target points into a corresponding one of the x, y, or z dimensions aligned with the normal vector, where x, y, and z represent the horizontal axis, vertical axis, and height axis defined by the system, respectively.

[0009] Another aspect of this disclosure includes employing iterative nearest point (ICP) analysis to determine the initial transformation between multiple source points and multiple target points.

[0010] Another aspect of this disclosure includes an initial transformation that is a translational transformation between multiple source points and multiple target points, and a rotational transformation between multiple source points and multiple target points.

[0011] Another aspect of this disclosure includes determining the association between source points and target points by determining the quadratic distance between source points and target points based on the x, y, and z dimensions, for multiple source points.

[0012] Another aspect of this disclosure includes determining the associated costs between multiple source points and multiple target points based on the root mean square (RMS) calculation of one of the selected dimensions of x, y, and z.

[0013] Another aspect of this disclosure includes determining a new transformation based on the x, y, and z dimensions, which minimizes the associated costs in the x, y, and z dimensions.

[0014] Another aspect of this disclosure includes updating the source points in the x, y, and z dimensions based on a new transformation.

[0015] Another aspect of this disclosure includes a space sensor that is one of a LiDAR sensor, a radar sensor, or a digital camera.

[0016] Another aspect of this disclosure includes a system that is a vehicle, wherein the system determines the position of the vehicle in the x, y, and z dimensions based on a spatial transformation between multiple source points and multiple target points.

[0017] Another aspect of this disclosure includes spatial sensors being arranged to monitor road sections that are close to and in front of the vehicle.

[0018] Another aspect of this disclosure includes vehicles with advanced driver assistance systems (ADAS), and controlling the driving automation state associated with ADAS based on spatial transformations between multiple source points and multiple target points.

[0019] This disclosure provides the following technical solutions:

[0020] 1. A system for determining image spatial transformation / image point cloud registration, comprising:

[0021] Space sensors and controllers;

[0022] The spatial sensors are arranged to capture multiple point clouds of the field of view;

[0023] The controller communicates with the space sensor, and the controller has an instruction set that can execute the following:

[0024] Multiple source points associated with the first point cloud are identified via the spatial sensor;

[0025] Multiple target points associated with the second point cloud are identified via the spatial sensor;

[0026] Perform normal vector calculation for each of the plurality of target points;

[0027] Based on the normal vector calculation, the plurality of target points are classified into one of the x, y, or z dimensions;

[0028] Determine the initial transformation between the plurality of source points and the plurality of target points;

[0029] For each of the x, y, and z dimensions:

[0030] For the plurality of source points, determine the association between the plurality of source points and the plurality of target points.

[0031] Determine the relevant costs between the plurality of source points and the plurality of target points.

[0032] A new transformation is determined based on the x, y, and z dimensions and the associated costs between the multiple source points and the multiple target points.

[0033] Update the source points in the x, y, and z dimensions based on the new transformation; and

[0034] Based on the updated source points in the x, y, and z dimensions, the spatial transformation between the plurality of source points and the plurality of target points is determined.

[0035] According to the system described in technical solution 1, the instruction set is executable to calculate the normal vector for each of the plurality of target points, including:

[0036] For each target point, the normal vector is calculated using the surrounding target points.

[0037] Estimate the surface plane of the target point, and

[0038] Estimate the normal vector of the surface plane.

[0039] According to the system of technical solution 1, the instruction set capable of executing to classify the plurality of target points into one of the x, y, or z dimensions based on the normal vector includes such an instruction set: which is capable of executing to classify each of the plurality of target points into a corresponding one of the x, y, or z dimensions aligned with the normal vector, wherein x, y, and z represent the lateral axis, longitudinal axis, and height axis defined by the system, respectively.

[0040] According to the system of technical solution 1, the instruction set capable of executing to determine the initial transformation between the plurality of source points and the plurality of target points includes an instruction set capable of executing to determine the initial transformation using iterative nearest point (ICP) analysis.

[0041] According to the system described in technical solution 4, the initial transformation includes translation transformation between the plurality of source points and the plurality of target points, and rotation transformation between the plurality of source points and the plurality of target points.

[0042] According to the system of technical solution 1, the instruction set that can be executed to determine the association between the source points and the target points for the plurality of source points includes determining the quadratic distance between the source points and the target points based on the x, y and z dimensions.

[0043] According to the system of technical solution 1, the instruction set is capable of performing root mean square (RMS) calculations based on selected dimensions in x, y, and z to determine the associated costs between the plurality of source points and the plurality of target points.

[0044] According to the system of technical solution 1, the instruction set is capable of executing a new transformation to determine the relevant costs of minimizing the x, y, and z dimensions based on the x, y, and z dimensions.

[0045] According to the system of technical solution 1, the instruction set is capable of being executed to update the source points in the x, y, and z dimensions based on the new transformation.

[0046] According to the system described in technical solution 1, the space sensor includes a LiDAR sensor.

[0047] According to the system described in technical solution 1, the space sensor includes a radar sensor.

[0048] According to the system described in technical solution 1, the space sensor includes a digital camera.

[0049] According to the system of technical solution 1, the system includes a vehicle; and the system determines the position of the vehicle in the x, y and z dimensions based on the spatial transformation between the plurality of source points and the plurality of target points.

[0050] According to the system described in technical solution 13, the space sensor is arranged to monitor road sections that are close to and in front of the vehicle.

[0051] According to the system of technical solution 13, the vehicle has an advanced driver assistance system (ADAS) and also includes an instruction set that can be executed to control the driving automation state associated with the ADAS based on spatial transformations between the plurality of source points and the plurality of target points.

[0052] A method for determining image spatial transformation, the method comprising:

[0053] Multiple point clouds are captured from the field of view via spatial sensors;

[0054] Identify multiple source points associated with the first point cloud;

[0055] Identify multiple target points associated with the second point cloud;

[0056] The controller performs normal vector calculation for each of the plurality of target points;

[0057] Based on normal vector calculation, the multiple target points are classified into one of the x, y, or z dimensions;

[0058] Determine the initial transformation between the plurality of source points and the plurality of target points; and

[0059] For each of the x, y, and z dimensions:

[0060] For the plurality of source points, determine the association between the plurality of source points and the plurality of target points.

[0061] Determine the relevant costs between the plurality of source points and the plurality of target points.

[0062] A new transformation is determined based on the x, y, and z dimensions and the associated costs between the multiple source points and the multiple target points.

[0063] Update the source points in the x, y, and z dimensions based on the new transformation; and

[0064] Based on the updated source points in the x, y, and z dimensions, the spatial transformation between the plurality of source points and the plurality of target points is determined.

[0065] According to the method described in technical solution 16, performing normal vector calculation for each of the plurality of target points includes:

[0066] For each target point, the normal vector is calculated using the surrounding target points.

[0067] Estimate the surface plane of the target point, and

[0068] Estimate the normal vector of the surface plane.

[0069] According to the method described in technical solution 17, classifying the plurality of target points into one of the x, y, or z dimensions based on normal vector calculation includes classifying each of the plurality of target points into a corresponding one of the x, y, or z dimensions aligned with the normal vector, wherein x, y, and z represent the horizontal axis, the vertical axis, and the height axis, respectively.

[0070] According to the method described in technical solution 16, determining the association between the source point and the target point for the plurality of source points includes determining the quadratic distance between the source point and the target point based on x, y and z dimensions, wherein the x, y and z dimensions include the root mean square (RMS) calculation of one of the selected dimensions among the x, y and z dimensions.

[0071] The foregoing summary is not intended to represent every possible embodiment or aspect of this disclosure. Rather, the foregoing summary is intended to illustrate some novel aspects and features disclosed herein. The foregoing features and advantages, as well as other features and advantages, of this disclosure will become apparent from the following detailed description of representative embodiments and modes of implementing this disclosure when taken in conjunction with the accompanying drawings and appended claims. Attached Figure Description

[0072] One or more embodiments will now be described by way of example with reference to the accompanying drawings, wherein:

[0073] Figure 1 A side view of a vehicle with a space monitoring system according to the present disclosure is schematically illustrated.

[0074] Figure 2 A flowchart illustrating the local dimension iterative nearest point analysis according to this disclosure is schematically shown.

[0075] The accompanying drawings are not necessarily drawn to scale and may present slightly simplified representations of various preferred features of the present disclosure, including, for example, specific dimensions, orientations, positions, and shapes. Details associated with such features will be determined in part by the specific intended application and usage environment. Detailed Implementation

[0076] As described and illustrated herein, the components of the disclosed embodiments can be arranged and designed in a variety of different configurations. Therefore, the following detailed description is not intended to limit the scope of the claimed disclosure, but is merely representative of possible embodiments therein. Furthermore, although numerous specific details are set forth in the following description to provide a thorough understanding of the embodiments disclosed herein, some embodiments may be practiced without some of these details. Additionally, for clarity, certain technical materials understood in the relevant art have not been described in detail to avoid unnecessarily obscuring this disclosure.

[0077] These figures are simplified and not to precise scale. For convenience and clarity only, directional terms (such as longitudinal, transverse, top, bottom, left, right, up, above, above, below, lower, rear, and front) may be used with respect to the figures. These and similar directional terms should not be construed as limiting the scope of this disclosure. Furthermore, as shown and described herein, this disclosure can be practiced in the absence of elements not specifically disclosed herein.

[0078] As used herein, the term "system" refers to mechanical and electrical hardware, software, firmware, electronic control components, processing logic, and / or processor devices that provide the aforementioned functionality, individually or in combination. This may include, but is not limited to, application-specific integrated circuits (ASICs), electronic circuits, processors (shared, dedicated, or grouped) that execute one or more software or firmware programs, memory containing software or firmware instructions, combinational logic circuits, and / or other components.

[0079] Referring to the accompanying drawings, the same reference numerals are used throughout several figures to correspond to the same or similar parts, consistent with the embodiments disclosed herein. Figure 1 A schematic side view of a vehicle 10 is shown, which is positioned on and capable of traversing a driving surface 70, such as a paved road surface. The vehicle 10 includes a space monitoring system 30 that communicates with a vehicle controller 50. The vehicle 10 may be, but is not limited to, a mobile platform for achieving the purposes of this disclosure in the form of a commercial vehicle, industrial vehicle, agricultural vehicle, bus, aircraft, ship, train, all-terrain vehicle, personal mobile device, robot, etc.

[0080] Vehicle 10 may also include other systems, including an in-vehicle navigation system 24, a computer-readable storage device or medium (memory) including a digital road map 23, a global positioning system (GPS) sensor 25, a human-machine interface (HMI) device 60, and an autonomous controller 65 and a telematics controller 75 in one embodiment.

[0081] In one embodiment, the spatial surveillance system 30 includes at least one spatial sensor 34 and associated systems, as well as a spatial surveillance controller 35, arranged to monitor a visible area 32 in front of the vehicle 10. The spatial sensor 34, arranged to monitor the visible area 32 in front of the vehicle 10, includes, for example, a lidar sensor, a radar sensor, a digital camera, etc. In one embodiment, the spatial sensor 34 is mounted on the vehicle to monitor all or part of the visible area 32, thereby detecting nearby remote objects such as road features, lane markings, buildings, pedestrians, road signs, traffic lights and signs, other vehicles, and geographical features approaching the vehicle 10. The spatial surveillance controller 35 generates a digital representation of the visible area 32 based on data input from the spatial sensor. The spatial surveillance controller 35 can evaluate the input from the spatial sensor 34 to determine continuous point cloud and associated linear range vehicle data, relative speed, and the trajectory of the vehicle 10 considering each nearby remote object. The spatial sensor 34 can be located at different positions on the vehicle 10, including front corners, rear corners, rear sides, front sides, or center sides. In one embodiment, the space sensor may include a front radar sensor and a camera, although this disclosure is not limited thereto. The placement of the space sensor 34 allows the space monitoring controller 35 to monitor traffic flow, including nearby vehicles, intersections, lane markings, and other objects around the vehicle 10.

[0082] The lane sign detection processor (not shown) can use data generated by the space monitoring controller 35 to estimate the road. The space sensors of the vehicle space monitoring system 30 may include object localization sensing devices, including distance sensors, such as FM-CW (Frequency Modulated Continuous Wave) radar, pulse and FSK (Frequency Shift Keying) radar, lidar devices, and ultrasonic devices that rely on effects such as the Doppler effect to locate objects ahead. Possible object localization devices include charge-coupled device (CCD) or complementary metal-oxide-semiconductor (CMOS) video image sensors, and other cameras / video image processors that "observe" objects ahead, including one or more vehicles, using digital photography methods.

[0083] When space sensor 34 is a LiDAR (Light Detection and Ranging) device, it uses pulsed and reflected laser beams to measure the range or distance to an object. When space sensor 34 is a radar device, it uses radio waves to determine the range, angle, and / or velocity of an object. When space sensor 34 is a digital camera, it includes a 2D or 3D image sensor, a lens, and a controller, wherein the image sensor is an electro-optical device that uses a multi-dimensional array of photosensitive elements to convert an optical image into a pixelated digital representation. The camera controller is operatively connected to the image sensor to monitor the visible area 32.

[0084] The autonomous controller 65 is configured to implement autonomous driving or advanced driver assistance system (ADAS) vehicle functions. Such functions may include onboard control systems capable of providing a degree of driving automation. The terms "driver" and "operator" describe the person responsible for directing the operation of vehicle 10, whether actively involved in controlling one or more vehicle functions or directing autonomous vehicle operation. Driving automation may include a series of dynamic driving and vehicle operations. Driving automation may include a degree of automatic control or intervention associated with a single vehicle function, such as steering, acceleration, and / or braking, where the driver maintains full control of vehicle 10 continuously. Driving automation may include a degree of automatic control or intervention associated with the simultaneous control of multiple vehicle functions, such as steering, acceleration, and / or braking, where the driver maintains full control of vehicle 10 continuously. Driving automation may include simultaneous automatic control of vehicle driving functions including steering, acceleration, and braking, where the driver relinquishes control of the vehicle for a period of time during the journey. Driving automation may include simultaneous automatic control of vehicle driving functions, including steering, acceleration, and braking, where the driver relinquishes control of vehicle 10 throughout the journey. Driving automation includes hardware and controllers configured to monitor the spatial environment in various driving modes to perform various driving tasks during dynamic vehicle operation. As a non-limiting example, driving automation may include cruise control, adaptive cruise control, lane change warning, intervention and control, automatic parking, acceleration, braking, etc. As a non-limiting example, autonomous vehicle functions include adaptive cruise control (ACC) operation, lane guidance and lane keeping operation, lane changing operation, steering assist operation, object avoidance operation, parking assist operation, vehicle braking operation, vehicle speed and acceleration operation, vehicle lateral movement operation, such as as part of lane guidance, lane keeping, and lane changing operations, etc. Thus, braking commands can be generated by the autonomous controller 65 independently of the vehicle operator's actions and in response to autonomous control functions.

[0085] Operator controls may be included in the passenger compartment of vehicle 10, and by way of non-limiting example, may include a steering wheel, accelerator pedal, brake pedal, and operator input devices as elements of HMI device 60. Operator controls enable a vehicle operator to interact with and guide the operation of vehicle 10 in providing passenger transport functions. In some embodiments of vehicle 10, operator control devices including a steering wheel, accelerator pedal, brake pedal, gear selector, etc., may be omitted.

[0086] HMI device 60 provides human-machine interaction for guiding the operation of infotainment systems, GPS sensors 52, navigation systems 24, etc., and includes controllers. HMI device 60 monitors operator requests and provides information to the operator, including vehicle system status, service, and maintenance information. HMI device 60 communicates with and / or controls the operation of multiple operator interface devices, wherein the operator interface devices are capable of transmitting messages associated with an operation in one of the autonomous vehicle control systems. HMI device 60 may also communicate with one or more devices that monitor biometric data associated with the vehicle driver, including, for example, eye gaze position, posture, and head position tracking. For ease of description, HMI device 60 is depicted as a single device, but in embodiments of the system described herein, it may be configured as multiple controllers and associated sensing devices. Operator interface devices may include means capable of transmitting information that prompts operator action and may include electronic vision display modules, such as liquid crystal display (LCD) devices, head-up displays (HUDs), audio feedback devices, wearable devices, and haptic seats. Operator interface devices capable of prompting operator action are preferably controlled by or through HMI device 60. A head-up display (HUD) projects information reflected inside the vehicle's windshield into the operator's field of vision, including the confidence level associated with transmitting and operating an autonomous vehicle control system. The HUD can also provide augmented reality information such as lane position, vehicle path, direction, and / or navigation information.

[0087] The in-vehicle navigation system 24 uses a digital road map 25 to provide navigation support and information for the vehicle operator. The autonomous controller 65 uses the digital road map 25 to control the autonomous vehicle operation or ADAS vehicle functions.

[0088] Vehicle 10 may include a telematics controller 75, which includes a wireless telematics communication system capable of external communication, including communication with a communication network 90 having wireless and wired communication capabilities. The telematics controller 75 is capable of external communication, including short-range vehicle-to-vehicle (V2V) communication and / or vehicle-to-everything (V2x) communication, and may include communication with infrastructure monitors (e.g., traffic cameras). Alternatively or additionally, the telematics controller 75 has a wireless telematics communication system capable of short-range wireless communication to a handheld device, such as a mobile phone, satellite phone, or another telephone device. In one embodiment, the handheld device includes a software application that includes a wireless protocol for communicating with the telematics controller 75, and the handheld device performs external communication, including communication with a non-vehicle server 95 via the communication network 90. ​​Alternatively or additionally, the telematics controller 75 performs external communication directly by communicating with the external server 95 via the communication network 90.

[0089] The term "controller" and related terms such as microcontroller, control unit, processor, and similar terms refer to one or more combinations of application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), electronic circuits, central processing units (e.g., microprocessors), and related non-transitory memory components in the form of memory and storage devices (read-only, programmable read-only, random access, hard disk drives, etc.), indicated by memory 23. The non-transitory memory components are capable of storing machine-readable instructions in the form of one or more software or firmware programs or routines, combinational logic circuits, input / output circuitry and devices, signal conditioning and buffering circuitry, and other components accessible by one or more processors to provide the aforementioned functionality. Input / output circuitry and devices include analog-to-digital converters and related devices for monitoring inputs from sensors, wherein these inputs are monitored at a preset sampling frequency or in response to trigger events. Software, firmware, programs, instructions, control routines, code, algorithms, and similar terms mean a set of instructions executable by the controller, including calibration and lookup tables. Each controller executes a control routine to provide the desired functionality. Routines may be executed periodically, for example, once every 100 microseconds during ongoing operation. Alternatively, routines can be executed in response to the occurrence of a triggering event. Communication between the controller, actuator, and / or sensor can be achieved using a direct wired point-to-point link, a network communication bus link, a wireless link, or other suitable communication link. Communication includes the exchange of data signals in a suitable form, including, for example, electrical signals via a conductive medium, electromagnetic signals via air, optical signals via an optical waveguide, etc. Data signals can include discrete, analog, or digitized analog signals representing inputs from sensors, actuator commands, and communication between the controller. The term "signal" refers to a physically discernible indicator that conveys information and can be a suitable waveform (e.g., electrical, optical, magnetic, mechanical, or electromagnetic) capable of propagating through a medium, such as DC, AC, sine waves, triangle waves, square waves, vibrations, etc. Parameters are defined as measurable quantities that represent the physical properties of a device or other element, which can be discerned using one or more sensors and / or physical models. Parameters can have discrete values, such as "1" or "0," or can be infinitely variable values.

[0090] As used herein, the terms “dynamic” and “dynamically” describe steps or processes performed in real time, in which the state of parameters is monitored or otherwise determined during routine execution or between iterations of routine execution, and the state of parameters is updated periodically or periodically.

[0091] A point cloud is a set of data points in space, which can be generated from a reference. Figure 1The described spatial sensor 34 captures points. These points can represent 3D shapes or objects. Each point location has its own set of Cartesian coordinates (x, y, z), which are defined relative to a reference object (e.g., a vehicle). Point cloud registration is the process of aligning two or more 3D point clouds of the same scene into a common coordinate system so that they are aligned, by finding a rigid transformation from one point to another. The point cloud transformation has translational dimensions of x, y, z, and rotational dimensions of yaw, pitch, and roll.

[0092] One process for determining point cloud transformations is Iterative Nearest Point (ICP), a method for matching two datasets by minimizing the distance between two sets of points. In ICP, one point cloud (vertex cloud, i.e., the reference or target) remains fixed, while the other point cloud (i.e., the source) is transformed to best match that reference. ICP can be used in LiDAR registration to match point clouds for various techniques, such as vehicle motion estimation. ICP is also used in other fields, such as computer vision systems.

[0093] The minimization operates simultaneously in all six dimensions of translation (x, y, z) and rotation (yaw, pitch, roll), and can be performed using methods such as the Newton-Raphson method. In one embodiment, the ICP algorithm determines the transformation between two point clouds by minimizing the squared error between corresponding entities.

[0094] However, ICP registration performance may be limited and may not provide sufficient accuracy for all tasks. In the registration problem, errors can be inconsistent across the six dimensions. Some dimensions may not exhibit errors, while others may have considerable initial errors. ICP does not account for dimensional inconsistencies in the errors but performs estimations across all dimensions, which can reduce accuracy on low-error dimensions.

[0095] Thus, local dimensional iterative nearest-point analysis is used to determine the spatial transformation between two point clouds. During the optimization process, the optimization step size for each dimension is normalized according to the standard deviation of the dimensional error (STD), thereby taking into account the error differences in each dimension and ignoring dimensions with zero STD error.

[0096] Figure 2An embodiment of a Local Dimension Iterative Closest Point (PD-ICP) routine 200 for determining spatial transformations or registrations between consecutively captured point clouds is schematically illustrated, where the spatial transformation is defined by translation (x, y, z) dimensions and rotation (roll, pitch, yaw) dimensions. The PD-ICP routine 200 is shown as a set of boxes in a logic flowchart, representing a sequence of operations that can be implemented in hardware, software, or a combination thereof. In the context of software, the boxes represent computer instructions that perform the operations when executed by one or more processors. For ease of illustration and clarity, refer to... Figure 1 The system 100 shown describes this method.

[0097] Table 1

[0098] box Box content 201 Source 202 Target point 210 Calculate the normal vector for the target point 211 Classify target points 220 Determine the initial transformation 221 Execute a loop along the x, y, and z dimensions 222 Determine the association 223 Calculate the cost function (RMS) 224 Select new transformation 225 Update source 230 Determine spatial transformation

[0099] The PD-ICP routine 200 can be executed as follows. The steps of the PD-ICP routine 200 can be executed in a suitable order, and are not limited to those specified in the reference. Figure 2 The order of description.

[0100] The inputs to the PD-ICP routine 200 include a source point 201 and a target point 202. The target point 202 represents a data point associated with a first fixed point cloud captured by the space sensor 34, which represents the FOV 32 at a first time point, and the source point 201 represents a second point cloud captured by the space sensor 34, which represents the FOV 32 at a subsequent second time point.

[0101] Calculate the normal vector for target point 202 (step 210). This calculation includes calculating the normal vector for each point of target point 202. The normal vector uses surrounding points to estimate the surface plane to which the point belongs, and the normal vector of that plane.

[0102] The normal vector at each point Defined as ,in It is the x-axis value of the normal vector. It is the y-axis value of the normal vector, and It is the z-axis value of the normal vector. The terms x, y, and z represent the lateral axis, longitudinal axis, and height axis, respectively, as defined by the vehicle or other platform on which system 100 is implemented.

[0103] The target point is classified into one of the three translation dimensions x, y, and z (step 211). Each point is classified into the dimension aligned with its normal vector, or is removed when the orientation difference is higher than a threshold, in order to filter out points that are weakly correlated with the normal vector.

[0104] Dimension selection is defined as follows:

[0105] [1].

[0106] In one embodiment, the filtering criterion is the arccosine of the point relative to the corresponding axis (cos...). -1 When the calculated arc is large enough, the normal vector is considered to be associated with the axis. The arc can be determined by the following formula:

[0107] [2]

[0108] in:

[0109] It is a point normal vector;

[0110] n i (For example, n) x n y n z One of them is the normal vector value at the i-axis;

[0111] e d It is the unit vector of axis d; and

[0112] The arc parameter that can be calibrated at the threshold, for example, 60°.

[0113] The optimization process initially determines the initial transformation between the source point 201 and the target point 202. The initial transformation can be predetermined or determined using the Iterative Closest Point (ICP) algorithm (step 220). The initial transformation includes translation (x, y, z) and rotation (yaw, pitch, roll). The source point is transformed accordingly, and a set of updated source points is calculated for each transformation.

[0114] The optimization process is carried out by performing three iterations, in which each of the x, y and z dimensions is iterated separately (step 221).

[0115] In each iteration, the point associated with the nearest point in the target is calculated (step 222) as the quadratic distance D, as follows:

[0116] [3]

[0117] in:

[0118] It is the source point; and

[0119] It is the target point.

[0120] The cost function of the correlation between the source and target points is calculated using the root mean square (RMS) of the points classified into the dimension (x, y, or z) of the current iteration (step 223), i.e., for each transformation:

[0121] [4]

[0122] in:

[0123] C i These are the relevant costs;

[0124] i is the transformation exponent;

[0125] K id It is the set of points of dimension d associated with transformation I;

[0126] p t It is the target point; and

[0127] p s It is the source point.

[0128] As described below, a new transformation is selected (step 224). Each transformation includes a transformation of each of the x, y, z, and rotational dimensions of yaw, pitch, and roll. The updated dimension is the translational dimension d of the current iteration. For rotational dimensions, dimensions perpendicular to d are excluded. Therefore, for x, y, and z, roll, pitch, and yaw are excluded respectively. The optimization process is then used based on the relevant cost. A new transformation is obtained (Equation 4), the optimization process of which is such as Nelder-Mead simplex gradient descent, which reduces the associated costs. minimize.

[0129] Use the learning rate factor to adjust dimension updates:

[0130] [5]

[0131] in:

[0132] l d It is the learning rate factor of dimension d, where the update step size is multiplied by this factor; and

[0133] STD(d) is the standard deviation of the dimension error d associated with the initial transformation.

[0134] The new transformation is used to update the source point, i.e., x, y, z (step 225), and process 200 iterates over the updated point.

[0135] After performing the process with a predefined number of iterations for dimensions x, y, z, the final spatial transformation is generated and output, wherein the final spatial transformation has translation (x, y, z) dimensions and rotation (roll, pitch, yaw) dimensions (step 230).

[0136] The final spatial transformation result can be used to dynamically update the position of vehicle 10 in real time.

[0137] This information can be used to verify information from GPS, or as a substitute for information from GPS.

[0138] The PD-ICP algorithm 200 provides a dimension-selective optimization method to determine the final spatial transformation between two point clouds.

[0139] When in reference Figure 1 When used in the embodiment of the ADAS-capable vehicle 10 described, the vehicle's motion can be advantageously guided based on the final spatial transformation determined using the PD-ICP algorithm 200. The autonomous controller 65 is capable of controlling driving automation states associated with ADAS, such as acceleration, steering, or braking, based on the final spatial transformation determined using the PD-ICP algorithm 200.

[0140] Flowcharts and block diagrams within flowcharts illustrate the architecture, functionality, and operation of possible implementations of systems, methods, and computer program products according to various embodiments of this disclosure. In this regard, each block in a flowchart or block diagram may represent a module, code segment, or code portion, including one or more executable instructions for implementing a specified logical function. It will also be noted that each block in the block diagrams and / or flowchart illustrations, and combinations of blocks in the block diagrams and / or flowchart illustrations, may be implemented by a system based on special-function hardware, or a combination of special-function hardware and computer instructions, that performs the specified function or action. These computer program instructions may also be stored in a computer-readable medium that can instruct a computer or other programmable data processing apparatus to operate in a particular manner, such that the instructions stored in the computer-readable medium produce an article of writing that includes a set of instructions implementing the functions / actions specified in one or more blocks of the flowcharts and / or block diagrams.

[0141] The detailed description and accompanying drawings are intended to support and describe this teaching, but the scope of this teaching is defined only by the claims. While some preferred modes and other embodiments for carrying out this teaching have been described in detail, various alternative designs and embodiments exist to practice the teaching as defined in the appended claims.

Claims

1. A system for determining image space transformation / image point cloud registration, comprising: a spatial sensor and a controller; the spatial sensor arranged to capture a plurality of point clouds of a field of view; the controller in communication with the spatial sensor, the controller having a set of instructions executable to: identify, via the spatial sensor, a plurality of source points associated with a first point cloud; identify, via the spatial sensor, a plurality of target points associated with a second point cloud; perform a normal vector calculation for each of the plurality of target points; classify the plurality of target points into one of an x, y, or z dimension based on the normal vector calculation; determine an initial transformation between the plurality of source points and the plurality of target points; for each of the x, y, and z dimensions: for the plurality of source points, determine an association between the plurality of source points and the plurality of target points, based on the association, determine a cost of correlation between the plurality of source points and the plurality of target points, determine a new transformation based on the x, y, and z dimensions and the cost of correlation between the plurality of source points and the plurality of target points, and update the source points of the x, y, and z dimensions based on the new transformation; and based on the updated source points of the x, y, and z dimensions, determine a spatial transformation between the plurality of source points and the plurality of target points for real-time dynamic updating of the positions of the source points.

2. The system of claim 1, wherein, the set of instructions executable to perform a normal vector calculation for each of the plurality of target points includes: for each of the target points, calculate a normal vector using surrounding ones of the target points, estimate a surface plane of the target point, and estimate a normal vector of the surface plane.

3. The system of claim 1, wherein, the set of instructions executable to classify the plurality of target points into one of an x, y, or z dimension based on the normal vector calculation includes a set of instructions executable to classify each of the plurality of target points into a respective one of the x, y, or z dimensions aligned with the normal vector, wherein x, y, and z represent lateral, longitudinal, and height axes, respectively, defined by the system.

4. The system of claim 1, wherein, the set of instructions executable to determine an initial transformation between the plurality of source points and the plurality of target points includes a set of instructions executable to employ an iterative closest point (ICP) analysis to determine the initial transformation.

5. The system of claim 4, wherein, the initial transformation includes a translational transformation between the plurality of source points and the plurality of target points and a rotational transformation between the plurality of source points and the plurality of target points.

6. The system of claim 1, wherein, the set of instructions executable to determine an association between the source points and the target points for the plurality of source points includes determining a squared distance between the source points and the target points based on the x, y, and z dimensions.

7. The system of claim 1, wherein, the set of instructions executable to determine a cost of correlation between the plurality of source points and the plurality of target points based on a root mean square (RMS) calculation of selected ones of the x, y, and z dimensions.

8. The system of claim 1, wherein, the set of instructions executable to determine a new transformation that minimizes the cost of correlation of the x, y, and z dimensions based on the x, y, and z dimensions.

9. The system of claim 1, wherein, the set of instructions executable to update the source points of the x, y, and z dimensions based on the new transformation.

10. The system of claim 1, wherein, the spatial sensor includes a LiDAR sensor.

11. The system of claim 1, wherein, The spatial sensor comprises a radar sensor.

12. The system of claim 1, wherein, The spatial sensor comprises a digital video camera.

13. The system of claim 1, wherein, The system comprises a vehicle; and wherein the system determines a position of the vehicle in x, y, and z dimensions based on a spatial transformation between the plurality of source points and the plurality of target points.

14. The system of claim 13, wherein, The spatial sensor is arranged to monitor a road segment proximate and in front of the vehicle.

15. The system of claim 13, wherein, The vehicle has an advanced driver assistance system (ADAS) and further comprises a set of instructions executable to control a driving automation state associated with the ADAS based on a spatial transformation between the plurality of source points and the plurality of target points.

16. A method for determining an image spatial transformation, the method comprising: capturing a plurality of point clouds of a field of view via a spatial sensor; identifying a plurality of source points associated with a first point cloud; identifying a plurality of target points associated with a second point cloud; performing a normal vector calculation for each of the plurality of target points via a controller; classifying the plurality of target points into one of an x, y, or z dimension based on the normal vector calculation; determining an initial transformation between the plurality of source points and the plurality of target points; and for each of the x, y, and z dimensions: for the plurality of source points, determining an association between the plurality of source points and the plurality of target points, based on the association, determining a correlation cost between the plurality of source points and the plurality of target points, determining a new transformation based on the x, y, and z dimensions and the correlation cost between the plurality of source points and the plurality of target points, and updating the source points of the x, y, and z dimensions based on the new transformation; and based on the updated source points of the x, y, and z dimensions, determining a spatial transformation between the plurality of source points and the plurality of target points for real-time dynamic updating of the source points' positions.

17. The method of claim 16, wherein, performing a normal vector calculation for each of the plurality of target points comprises: for each target point, calculating a normal vector using surrounding ones of the target points, estimating a surface plane of the target point, and estimating a normal vector of the surface plane.

18. The method of claim 17, wherein, classifying the plurality of target points into one of an x, y, or z dimension based on the normal vector calculation comprises classifying each of the plurality of target points into a respective one of the x, y, or z dimensions that aligns with the normal vector, wherein x, y, and z represent a lateral axis, a longitudinal axis, and a height axis, respectively.

19. The method of claim 16, wherein, determining the association between the source points and the target points for the plurality of source points comprises determining a quadratic distance between the source points and the target points based on the x, y, and z dimensions, the x, y, and z dimensions comprising a root mean square (RMS) calculation of a selected one of the x, y, and z dimensions.

Images