Custom 3D LiDAR & drivers
Objective
Build a hobby-grade 3D scanner from a 2D LiDAR. Over five months, I developed a system that rotates an RPLIDAR C1 along an additional axis using an MKS Servo 42C closed-loop stepper motor and custom 3D printed mounts.
Custom C++ drivers
I chose C++ for its performance potential and low-level hardware control. After experimenting with existing RPLIDAR libraries, I wrote drivers for both devices to gain the control needed to modify scan capture and benchmark speed improvements. The drivers discover the devices over USB, communicate over serial (UART), construct command packets, and parse device responses.
- Packet synchronization, checksum validation, and recovery from malformed or misaligned data.
- Bitwise decoding of distance, angle, quality, and scan-start information from the LiDAR byte stream.
- Protocol-specific byte ordering, including the motor's big-endian response format.
- Motor positioning using encoder feedback, angular calculations, and the configured microstepping.
Synchronized scanning & reconstruction
The scan starts by moving the motor to zero and initializing the scan state. At each position, the software verifies motor feedback, captures a valid 2D slice, and stores it with the measured angle. It then advances by 1° until 180 slices have been captured. Scan boundaries and measurement validation keep each slice tied to the correct motor position.
Coordinate transformations convert polar LiDAR measurements into 3D Cartesian coordinates, accounting for the motor's 180° rotation. The resulting point cloud is visualized in Open3D.
Latest results
- Approximately 56,000 captured points in a recognizable room reconstruction.
- A complete scan in roughly 5 minutes, improved from 8–10 minutes in earlier versions.
- My highest-fidelity capture to date, shown in the first video.
Next steps
Occlusion, surface reflectivity, and measurement noise still leave gaps or imperfect regions. I plan to improve scanning throughput, refine noise filtering, and experiment with adaptive angular sampling to reduce unnecessary measurements.
Links