Ezward
commented
3 years ago Integrating 2D Lidar data into Donkeycar
The 2D Lidar data
A spinning lidar captures data in 360 degree arc around it. It provides each data point as a distance from the lidar and the angle at which that reading was taken. It has some maximum working range. That range can be thought of as the radius of a circle where the lidar is at the center of that circle. Beyond that range we may actually get some readings, but we treat those a noise.
The lidar's 'zero' angle is relative to itself and so is all the data that it provides to us (it's super narcissistic). But we care about where the points are relative to the vehicle that the lidar is mounted on. If we treat directly forward as 'zero' angle for the vehicle, then we need to know the offset of the zero angle on the lidar relative to the forward direction of the vehicle. We can then adjust the angle component of each data point provided by the lidar to turn it into an angle relative to the vehicle. It may well be that the way the lidar is mounted that the zero angle of the lidar exactly coincides with forward on the vehicle, but we should include this offset (which in that case would be zero) anyway so we have a more general model that can be applied to other lidars that may not have their zero in the same place.
Plotting the 2D Lidar data
So once we turn the lidar data into a distance and angle relative to the vehicle, then we can use trigonometry to calculate (x,y) coordinates relative to the vehicle; the vehicle is defined as being at (0,0). We would likely want to set a bounding box in the 2D data space, so we could clip data we don't want. We would also want the dimensions of the bitmap we are to render onto.
- Use simple trigonometry to convert the polar coordinate 2D data to an cartesian value in data space.
- throw away any values not within the data bounding box
- map data space points image space and plot as a pixel (size of pixel should be configurable).
A complication
Another thing to think about is the skew inherent in the data taken from a moving vehicle. Remember that the each data point provided by the lidar is given as a distance from the lidar and the angle at which the lidar was pointing when it took the reading. But if the lidar is on a moving vehicle, then the position from which each point is taken is changing. The lidar has some rate at which it can make a full 360 degree sweep. This is typically 5 to 10 hz. If the vehicle is stopped, then all data points are from a circle with the same center. However, as the vehicle moves, each point is taken from a different location. If the vehicle is moving 1 meter per second, which is pretty slow for a race car, and the lidar is spinning at 5hz, then the lidar has moved 1/5 of a meter between taking the first point in a sweep and the last point. There is additional skew that happens if the vehicle is turning.
A solution
If we know the vehicle's position and orientation as it moves, then we can simply adjust each point provide by the lidar using the vehicle's position and orientation at the time that point was taken.
If we have good odometry, then we can apply a kinematic model that estimates the vehicles position and orientation as it moves. Such estimates are good over short distances and we only need them to be good over a meter or so. If we have wheel encoders or an encoder on the drive motor, then we can use those. If we have an IMU, then we can also use that for the same purpose. Having both would be ideal.
But what if we don't have good odometry? That is typical of a Donkeycar. We know the throttle value and steering value, but we don't actually know the speed of the car. However, with a little calibration we can roughly estimate the speed of the vehicle from the throttle value. The calibration would involve taking some measurements over a known distance at different constant throttle values and measuring the time to traverse the distance. Then we can do interpolation to estimate the speed. That would provide a very rough estimate. It is affected by the particular surface the vehicle is on and by the battery charge level. If the vehicle is changing speeds, then this model also does not take into account the lag in acceleration and deceleration. However, it is better than not doing any adjustment of the data.
We also need to calibrate the steering angle as well; measure the radius of a full right and full left turn; then we can interpolate to estimate turn angle given a steering value.
With those two things in place, we can then use the speed and steering angle as input to a kinematic model to estimate the vehicle's relative position and orientation over time.
A Donkeycar 2D Lidar pipeline
Acquiring the data from the 2D lidar and transforming it into data useful to the CNN involves integrating a number of new parts into the vehicle pipeline. 2D Lidar pipeline involves 4 parts that work to gather data from the 2D Lidar, adjust for the changing position of the vehicle and render it to a bitmap so it can be saved to the tub.
-
Encoder Part: If we have a motor encoder, then this part reads the motor encoder and writes the value and the time in nanoseconds at which the data was read. Donkeycar does have an encoder.py part that assumes that we have an arduino or similar microcontroller to read the encoder. That part defines how encoder data is written to the pipeline. We should try to use that if we can.
-
Kinematics Part: This part takes as input the encoder reading (if we have an encoder), the throttle value and the steering value. It uses the encoder reading if available or throttle otherwise, and the steering value along with other relevant configuration and calibration values and maintains an estimate of the vehicle's position and orientation. It outputs the position and orientation to the data pipeline along with a high resolution timestamp (taken from the encoder value if available). Donkeycar does not currently have this part, but it would be a great part to add; we could use it beyond the 2D Lidar pipeline for things like navigating around an obstacle.
-
2D Lidar Part: This part takes as input the kinematic data (position and orientation of the vehicle). This part also reads data from the 2D lidar. It maintains a queue of all the points in the last 360 degrees of data; both the raw data received by the lidar and an adjusted value for the distance and angle from the vehicle that takes into account the kinematic data. Each point should also be marked with a time in nanoseconds at which it was read. The size of the data queue depends upon the resolution of the lidar (which should be configurable). The part outputs the queue into the vehicle's data pipeline so it can be processed by other parts and possibly saved to the tub.
-
Lidar Imaging Part: This part uses the 2D lidar queue, a lidar data bounding box, an input image and an image bounding box and renders the 2D lidar data as pixels on the image. This image is output to the pipeline so later it can be saved to the tub. I'm suggesting keeping the part that reads the lidar data separate from this image generation part so that a) we can implement different 2D lidar parts for different brands of Lidar without having to include the image code with them b) we can use the 2D lidar data in other ways (like obstacle detection) without outputing an image.
- The bounding box of lidar readings to be included in the image are configured in myconfig.py. The bounding box units would be in meters. So if our 2d lidar had a maximum range of 8 meters and we wanted all the data around the vehicle to be plotted, we would configure a bounding box (left, top, right, bottom) of (-8, 8, 8, -8) using cartesian coordinates and assuming the lidar as at (0,0). By specifying a bounding box of (-8, 8, 8, 0) we could ignore all the points behind the vehicle. Further, we could constrain the distance even more to eliminate data that is not important (that is too far away from the vehicle); for instance if we only cared about things up to 4 meters in front of the vehicle and 2 meters on each side we would specify this bounding box; (-2, 4, 2, 0)
- The image bouds are configured in myconfig. This represents that area on the input image where the lidar data would be drawn.
We also want a part that will convert the normal RGB camera images into a bird's eye view image using a camera calibration matrix. This bird's eye view would then be the input image for the Lidar Imaging Part and the resulting combined image would be input to the CNN. See a more detailed discussion below.
A Neural Network using 2D Lidar data
It may be that there are other neural network archictures that would use the 2D lidar data directly, but Donkeycar is built around a CNN that takes images in and outputs steering and throttle. So if we stick with that, then we want to use the 2D lidar as an image and use this image as an input into the CNN. I can think of two ways to do that. One is pretty simple and one is more complex, but likely better.
One way to use the Lidar data is to concatenate the 2D lidar image with the camera image we already have. So if we are capturing a 160x120 camera image, we would want our 2DLidar image to be the same width (120 pixels), then we would use OpenCV to 'stack' the two images and make them into a single image. That single image would then be the input to our CNN. Note that we would need to change the code that calculates the CNN to use the dimensions of this new image, not the dimensions of the camera image. Further, when we drive on autopilot, we need to do this concatenation step and use the concatenated image to infer the steering and throttle.
I think a model build on that might be ok, but it's hard to know without testing it. It definitely would slow things down because the image would be larger. Also, if you think about this, it's not super intuitive that this would work well. The two images are in two different 'spaces'; it would not be obvious to a person how these correlate and it might not be obvious to a CNN either.
Another way, which I think would be better, would be to overlay the 2D lidar image onto the camera image in a way where the lidar pixels are correctly placed in the image. Each lidar data point represents a point in space around the vehicle where something exists, because it reflected that light. We should be able to place the 2D lidar pixel onto the pixel in the camera image that represents the spot in 3D space that reflected that light.
I think there are two possible ways to do this; the first approach attempts to take the Lidar data and project it into the 3D space that the camera image represents. The second method instead projects the camera image into a bird's eye view and then overlays the 2D lidar data on it. These two alternatives are discussed in more detail.
Option A: we could project the Lidar data points onto the camera image, applying a 3D transform that positions the pixels correctly in the image. To do this we must calibrate the camera and use that calibration to understand how to project the points in the 2D lidar data into the camera's world view. Remember that each Lidar point represents a point in the 3D world space around the vehicle. In this option, we take that 3D point and calculate the corresponding pixel in the camera image and draw it there. This is kinda cool, but I don't think it is the best way to do this because of perspective; as points get farther away, they are likely to occupy the same pixel as a nearby point because of perspective. That means we start to lose data.
Option B: To avoid the perspective data loss issues, we could use the camera calibration values to reformat the camera image so it appears as a bird's eye view; like we are looking at it from above. We essentially reformat the image so it looks like it was taken from directly above the Donkeycar. Then it is basically trivial to overlay the 2D lidar data, because it already is like a bird's eye view. We merge the pixels from the 2D Lidar image into the bird's eye camera view. Even better, we can generate this bird's eye camera view before we render the 2D lidar image and rather then render a 2D lidar image to it's own bitmap, we draw the pixels directly on the bird's eye camera image. That would be faster because we can avoid merging two bitmaps.
I think this bird's eye image would make the 2D lidar data much more powerful in predicting steering angle and throttle (assuming we slow down for obstacles). This approach would require another part; a part that takes the camera calibration data and the camera image and generates a bird's eye camera image. We would also make the Lidar Image Part accept an input image and rather than image dimensions, it would get a second bounding box that represents the area on the input image (the bird's eye camera image) where the 2D lidar data should be drawn.
I'm getting pretty excited about this second approach. It has other benefits;
- We can use that bird's eye view and lidar data to do a good job of obstacle detection. Since the bird's eye view will remove camera distortion, we can use it to calculate paths we want to drive in the world.
- In an indoor setting with walls (think a school), the 2D lidar alone could probably be used to calculate a decent network. That would be a cool demo; create a network that drives around the perimeter of a room or the outside of a building by following a wall. You could use it to solve a maze using wall following.
This approach does require another calibration step; calibrating the camera. This is a well known process and there exists plenty of code samples in Python to generate the necessary calibration data. Further, there is plenty of Python code out there to apply this calibration data to turn a camera image into a bird's eye view; this is a common process in many robotics projects. The bird's eye view work is broken out into issue 1096.
Heavy02011
TCIII
This is a discussion issue. The topic is how to integrate 2D lidar data, as from an RPLidar or YDLidar, into the DonkeyCar framework. It is likely that this will result in a number of following-on issues to actually implement the design that we decide upon. I'll start the discussion by adding a strawman as the first comment in this ticket.
So I realize there is a lot of work already done here. donkeycar/parts/lidar.py has code to read RPLidar, YDLidar and has code for plotting the data. It even has BreezySLAM. So when reading the strawman below, know that a bunch of this work is already done. I think the big piece we need to do is to integrate the camera and 2D lidar images so they can be fed to the CNN. Then we need to make sure our support for various lidar models and brands is working in that regard. Finally we may want to handle the issue of data skew while moving so our scans are more accurate. Also I think we can optimize the RPLidar driver a little more; it can filter the data as it is collected and it can insert the data sorted so we can avoid doing that each time we read the data.