kestrelm / cuda-particle-filter Goto Github PK

The biggest limitation of particle filters is that for a large amount of particles, CPU implementation is incredibly costly. To compensate for this, historically SLAM algorithms relied heavily on good robot odometry to create a prior on the distribution using dead-reckoning. However, as mobile graphics processors become more viable for robotic applications increasing the particle count becomes a great way to improve estimation accuracy without siginificant runtime increase. In fact, the particle filter algorithm for SLAM can be considered embarassingly parallel in implementation, and accurate results can be achieved from visual sensor data only, without any need to develop a prior with robot odometry.

The Particle Filter algorithm can be broken into six basic steps:

1. Disperse all particles with gaussian random noise.
2. For each particle, calculate which grid cells the sensor detects an obsticle in, and sum these values.
3. Select the highest scoring particle as the new position for the next time step, and adjust all particle weights relative to the distribution of scores calculated in step 2.
4. Update the map by increasing the value of detected collisions, and decreasing the value of cells between the current position and each measured collision.
5. Resample the particles proportionally to their weight distribution.
6. Repeat steps 1 through 5 for each successive sensor input.

While there is little qualatative analysis that can be used to evaluate the accuracy of the final results without having the floorplan for buildings being mapped, the above images show very clear floor plans for five different data sets. All sensor data was performed with a lidar with unknown sensor noise, a range of up to 5 meters over a 270 degree arc in .25 degree intervals. While IMU and odometry sensor data was available to use for improving accuracy, the maps above clearly show that vision only SLAM is entirely viable when CPU runtime is not the primary bottleneck. Below, the comparison between 50, 5,000, and 50,000 particles can be seen in the final map to evaluate loop closure and returning to the original state.

For specific time imrpovements, the table below shows that for 5000 particles, we see massive time improvements in the measurement update step when evaluating all particles as would be expected. The parallel implementation shows a near 150 fold improvement overall. In terms of total processing speed, the 5000 particle benchmark ran around 170 Hz. Since sensor data is coming in at a 40 Hz rate, this allows for processing a large sample space in real time with additional headroom for running other operations on the machine. Even at 50,000 particles, the GPU implementation ran at ~20 Hz, which would be perfectly acceptable if the sensor data was subsampled every 2 or 3 readings, however, accuracy improvements appear negligible for loop-closure.

Note: unfortunately NVidia NSight Analysis does not properly load matlab libraries, so detailed thread performance could not be without refactoring the data import code

This code requires the matlab runtime library for C++ to be installed on the machine. Make sure that the matlab library path is included in Project>Properties>Configuration Properties>Debugging "Environment". E.G.:

PATH=%PATH%;C:\Program Files\MATLAB\R2015a\bin\win64;

Once compiled, the executable requires two input files: a text file specifying camera data and a matlab file containing lidar scans. See the 'data' folder for examples of both.

CMake note: Do not change any build settings or add any files to your project directly (in Visual Studio, Nsight, etc.) Instead, edit the src/CMakeLists.txt file. Any files you add must be added here. If you edit it, just rebuild your VS/Nsight project to make it update itself.

If you experience linker errors on build related to the compute capability during thrust calls, edit the project to include the CUDA library 'cudadevrt.lib'