Skip to content

Commit 697cdbf

Browse files
markarenmarkaren
authored
Add lidar (#325)
1 parent 4a7ac15 commit 697cdbf

File tree

8 files changed

+684
-0
lines changed

8 files changed

+684
-0
lines changed

‎README.md‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -249,6 +249,7 @@ set_languages("c++20")
249249
![Crane](doc/screenshots/crane.png)
250250
![FlyControls](doc/screenshots/fly.PNG)
251251
![Optimization](doc/screenshots/Optimization.PNG)
252+
![Lidar](doc/screenshots/lidar.png)
252253
![Animation](doc/screenshots/animation.png)
253254
![Water+sky](doc/screenshots/water_sky.png)
254255
![MotorController](doc/screenshots/motor_controller.PNG)

‎doc/screenshots/lidar.png‎

195 KB
Loading

‎examples/helpers/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2,3 +2,4 @@
22
add_example(NAME "helpers" WEB)
33
add_example(NAME "camera_helper" WEB)
44
add_example(NAME "depth_sensor" LINK_IMGUI WEB)
5+
add_example(NAME "lidar" LINK_IMGUI WEB)

‎examples/helpers/lidar.cpp‎

Lines changed: 184 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,184 @@
1+
2+
#include "threepp/extras/imgui/ImguiContext.hpp"
3+
#include "threepp/helpers/AxesHelper.hpp"
4+
#include "threepp/helpers/DepthSensor.hpp"
5+
#include "threepp/helpers/LidarSensor.hpp"
6+
#include "threepp/objects/Points.hpp"
7+
#include "threepp/threepp.hpp"
8+
9+
#include <cmath>
10+
#include <cstdlib>
11+
12+
using namespace threepp;
13+
14+
namespace {
15+
16+
// Build a simple scene: ground plane + scattered boxes
17+
void setupScene(Scene& scene) {
18+
// Ground
19+
auto ground = Mesh::create(
20+
BoxGeometry::create(30, 0.2f, 30),
21+
MeshStandardMaterial::create({{"color", Color(0x888888)}}));
22+
scene.add(ground);
23+
24+
// Random boxes
25+
std::srand(42);
26+
auto boxMat = MeshStandardMaterial::create({{"color", Color(0x4488cc)}});
27+
for (int i = 0; i < 20; ++i) {
28+
float w = 0.5f + (std::rand() % 100) / 50.f;
29+
float h = 0.5f + (std::rand() % 100) / 25.f;
30+
float d = 0.5f + (std::rand() % 100) / 50.f;
31+
auto box = Mesh::create(BoxGeometry::create(w, h, d), boxMat);
32+
box->position.set(
33+
(std::rand() % 240 - 120) / 10.f,
34+
h / 2.f + 0.1f,
35+
(std::rand() % 240 - 120) / 10.f);
36+
scene.add(box);
37+
}
38+
39+
// Lights
40+
scene.add(AmbientLight::create(0xffffff, 0.4f));
41+
auto dirLight = DirectionalLight::create(0xffffff, 1.f);
42+
dirLight->position.set(5, 10, 5);
43+
scene.add(dirLight);
44+
}
45+
46+
// Update a Points object's position and color attributes from a point cloud.
47+
// Colors are mapped by distance: near=green, far=red.
48+
void updatePointCloud(const Points& points, const std::vector<Vector3>& cloud,
49+
const Vector3& sensorPos, float maxDist) {
50+
51+
auto& geom = *points.geometry();
52+
auto* posAttr = geom.getAttribute<float>("position");
53+
auto* colAttr = geom.getAttribute<float>("color");
54+
55+
Color c;
56+
int i = 0;
57+
for (const auto& p : cloud) {
58+
posAttr->setXYZ(i, p.x, p.y, p.z);
59+
60+
c.setHSL(0.33f * (1.f - std::min(p.distanceTo(sensorPos) / maxDist, 1.f)), 1.f, 0.5f);
61+
colAttr->setXYZ(i, c.r, c.g, c.b);
62+
63+
++i;
64+
}
65+
66+
geom.setDrawRange(0, i);
67+
posAttr->needsUpdate();
68+
colAttr->needsUpdate();
69+
}
70+
71+
}// namespace
72+
73+
int main() {
74+
75+
Canvas canvas("Lidar", {{"antialiasing", 4}});
76+
GLRenderer renderer(canvas.size());
77+
78+
auto scene = Scene::create();
79+
scene->background = Color(0x111122);
80+
81+
auto camera = PerspectiveCamera::create(60, canvas.aspect(), 0.1f, 200.f);
82+
camera->position.set(0, 12, 18);
83+
84+
setupScene(*scene);
85+
86+
// --- Lidar sensor ---
87+
auto lidar = std::make_unique<LidarSensor>(LidarModel::OS0_128(), 512, 0.5f, 20.f);
88+
lidar->position.set(0, 2, 0);
89+
scene->add(*lidar);
90+
91+
OrbitControls controls{*camera, canvas};
92+
93+
// --- Point cloud visualisation ---
94+
const size_t maxPoints = 6 * std::pow(lidar->faceSize(), 2);
95+
auto pcGeom = BufferGeometry::create();
96+
pcGeom->setAttribute("position", FloatBufferAttribute::create(std::vector<float>(maxPoints * 3), 3));
97+
pcGeom->setAttribute("color", FloatBufferAttribute::create(std::vector<float>(maxPoints * 3), 3));
98+
pcGeom->getAttribute<float>("position")->setUsage(DrawUsage::Dynamic);
99+
pcGeom->getAttribute<float>("color")->setUsage(DrawUsage::Dynamic);
100+
101+
auto pcMaterial = PointsMaterial::create({{"size", 0.1f}, {"vertexColors", true}});
102+
auto points = Points::create(pcGeom, pcMaterial);
103+
points->layers.set(1);
104+
points->frustumCulled = false;
105+
scene->add(points);
106+
107+
108+
const char* modeNames[] = {"Dense Grid", "VLP-16", "HDL-32E", "OS1-64", "OS0-128"};
109+
int currentMode = 4;// start on OS0-128
110+
111+
auto changeLidar = [&](std::unique_ptr<LidarSensor> newLidar) {
112+
scene->remove(*lidar);
113+
newLidar->rangeNoise = lidar->rangeNoise;
114+
newLidar->position.copy(lidar->position);
115+
newLidar->rotation.copy(lidar->rotation);
116+
lidar = std::move(newLidar);
117+
scene->add(*lidar);
118+
};
119+
120+
bool senorDataOnly = false;
121+
ImguiFunctionalContext ui(canvas, [&] {
122+
ImGui::SetNextWindowPos({});
123+
ImGui::SetNextWindowSize({});
124+
ImGui::Begin("Settings");
125+
ImGui::Checkbox("Show senor data only", &senorDataOnly);
126+
ImGui::SliderFloat("Range noise", &lidar->rangeNoise, 0.f, 0.1f);
127+
128+
int prevMode = currentMode;
129+
ImGui::Combo("Mode", &currentMode, modeNames, 5);
130+
if (currentMode != prevMode) {
131+
switch (currentMode) {
132+
case 0: changeLidar(std::make_unique<LidarSensor>(64, 0.5f, 20.f)); break;
133+
case 1: changeLidar(std::make_unique<LidarSensor>(LidarModel::VLP16(), 512, 0.5f, 20.f)); break;
134+
case 2: changeLidar(std::make_unique<LidarSensor>(LidarModel::HDL32E(), 512, 0.5f, 20.f)); break;
135+
case 3: changeLidar(std::make_unique<LidarSensor>(LidarModel::OS1_64(), 512, 0.5f, 20.f)); break;
136+
case 4: changeLidar(std::make_unique<LidarSensor>(LidarModel::OS0_128(), 512, 0.5f, 20.f)); break;
137+
}
138+
}
139+
140+
ImGui::End();
141+
});
142+
143+
IOCapture capture;
144+
capture.preventMouseEvent = [] {
145+
return ImGui::GetIO().WantCaptureMouse;
146+
};
147+
canvas.setIOCapture(&capture);
148+
149+
canvas.onWindowResize([&](WindowSize size) {
150+
camera->aspect = size.aspect();
151+
camera->updateProjectionMatrix();
152+
renderer.setSize(size);
153+
});
154+
155+
Clock clock;
156+
std::vector<Vector3> cloud;
157+
std::vector<Color> colors;
158+
canvas.animate([&] {
159+
const float t = clock.getElapsedTime();
160+
161+
// Slowly sweep the sensor in yaw and pitch
162+
lidar->rotation.y = t * 0.4f;
163+
lidar->rotation.x = -0.4f + 0.25f * std::sin(t * 0.3f);
164+
165+
// Scan the scene and update the visualised point cloud
166+
points->visible = false;
167+
colors.clear();
168+
lidar->scan(renderer, *scene, cloud);
169+
points->visible = true;
170+
171+
Vector3 sensorWorld;
172+
lidar->getWorldPosition(sensorWorld);
173+
updatePointCloud(*points, cloud, sensorWorld, lidar->far());
174+
175+
if (senorDataOnly) {
176+
camera->layers.set(1);
177+
} else {
178+
camera->layers.enableAll();
179+
}
180+
181+
renderer.render(*scene, *camera);
182+
ui.render();
183+
});
184+
}

‎include/threepp/helpers/LidarModel.hpp‎

Lines changed: 77 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,77 @@
1+
#ifndef THREEPP_LIDARMODEL_HPP
2+
#define THREEPP_LIDARMODEL_HPP
3+
4+
#include <vector>
5+
6+
namespace threepp {
7+
8+
/**
9+
* Describes the beam pattern of a LiDAR sensor.
10+
*
11+
* Coordinate convention used by LidarSensor:
12+
* azimuth = 0 → sensor's forward direction (local -Z)
13+
* azimuth increases counter-clockwise when viewed from above (+Y)
14+
* elevation = 0 → horizontal
15+
* elevation > 0 → upward
16+
*/
17+
struct LidarModel {
18+
19+
/// Vertical angle of each laser beam in degrees.
20+
std::vector<float> elevationAngles;
21+
22+
/// Angular step between horizontal scan positions, in degrees.
23+
float azimuthResolution{0.2f};
24+
25+
/// Horizontal scan range [azimuthMin, azimuthMax), in degrees.
26+
/// Use -180 / 180 for a full 360-degree sweep.
27+
float azimuthMin{-180.f};
28+
float azimuthMax{180.f};
29+
30+
// ----------------------------------------------------------------
31+
// Preset sensor models
32+
// ----------------------------------------------------------------
33+
34+
/// Velodyne VLP-16: 16 beams, ±15° elevation at 2° steps, 0.2° azimuth.
35+
static LidarModel VLP16() {
36+
LidarModel m;
37+
m.elevationAngles.resize(16);
38+
for (int i = 0; i < 16; ++i)
39+
m.elevationAngles[i] = -15.f + i * 2.f;
40+
m.azimuthResolution = 0.2f;
41+
return m;
42+
}
43+
44+
/// Velodyne HDL-32E: 32 beams from -30.67° to +10.67° at ~1.33° steps, 0.1° azimuth.
45+
static LidarModel HDL32E() {
46+
LidarModel m;
47+
m.elevationAngles.resize(32);
48+
for (int i = 0; i < 32; ++i)
49+
m.elevationAngles[i] = -30.67f + i * (10.67f - (-30.67f)) / 31.f;
50+
m.azimuthResolution = 0.1f;
51+
return m;
52+
}
53+
54+
/// Ouster OS1-64: 64 beams, ±22.5° elevation (uniform), 0.35° azimuth.
55+
static LidarModel OS1_64() {
56+
LidarModel m;
57+
m.elevationAngles.resize(64);
58+
for (int i = 0; i < 64; ++i)
59+
m.elevationAngles[i] = -22.5f + i * (45.f / 63.f);
60+
m.azimuthResolution = 0.35f;
61+
return m;
62+
}
63+
64+
/// Ouster OS0-128: 128 beams, ±45° elevation (uniform), 0.35° azimuth.
65+
static LidarModel OS0_128() {
66+
LidarModel m;
67+
m.elevationAngles.resize(128);
68+
for (int i = 0; i < 128; ++i)
69+
m.elevationAngles[i] = -45.f + i * (90.f / 127.f);
70+
m.azimuthResolution = 0.35f;
71+
return m;
72+
}
73+
};
74+
75+
}// namespace threepp
76+
77+
#endif//THREEPP_LIDARMODEL_HPP

‎include/threepp/helpers/LidarSensor.hpp‎

Lines changed: 103 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,103 @@
1+
#ifndef THREEPP_LIDARSENSOR_HPP
2+
#define THREEPP_LIDARSENSOR_HPP
3+
4+
#include "LidarModel.hpp"
5+
#include "threepp/cameras/OrthographicCamera.hpp"
6+
#include "threepp/cameras/PerspectiveCamera.hpp"
7+
#include "threepp/core/Object3D.hpp"
8+
#include "threepp/renderers/GLRenderTarget.hpp"
9+
#include "threepp/scenes/Scene.hpp"
10+
11+
#include <array>
12+
#include <vector>
13+
14+
namespace threepp {
15+
16+
class ShaderMaterial;
17+
class GLRenderer;
18+
19+
/**
20+
* Simulates a full 360-degree LiDAR sensor using six 90-degree perspective
21+
* cameras oriented along the ±X, ±Y and ±Z axes.
22+
*
23+
* Two modes:
24+
* - Dense grid (default constructor): every pixel on every cube face becomes
25+
* a point — useful for visualisation and debugging.
26+
* - Model-based (LidarModel constructor): only the beams defined by the model
27+
* are sampled, matching the angular pattern of a real sensor (VLP-16, etc.).
28+
*
29+
* The sensor object must be in the scene (or have its parent chain updated)
30+
* before calling scan(), so all child camera world matrices are current.
31+
*/
32+
class LidarSensor: public Object3D {
33+
34+
public:
35+
// Gaussian range noise standard deviation in metres (0 = perfect sensor)
36+
float rangeNoise{0.02f};
37+
38+
/**
39+
* Dense-grid mode: every pixel on all six cube faces is returned.
40+
* @param faceSize Resolution of each cube face in pixels (square).
41+
* @param near Near clip plane in metres.
42+
* @param far Far clip plane / max range in metres.
43+
*/
44+
explicit LidarSensor(unsigned int faceSize, float near = 0.1f, float far = 100.f);
45+
46+
/**
47+
* Model-based mode: only beams defined by the LidarModel are sampled.
48+
* @param model Beam pattern (elevation angles + azimuth resolution).
49+
* @param faceSize Cube-face resolution. Should be ≥ 90/azimuthResolution
50+
* to avoid aliasing (e.g. 512 for 0.2° resolution).
51+
* @param near Near clip plane in metres.
52+
* @param far Far clip plane / max range in metres.
53+
*/
54+
LidarSensor(const LidarModel& model, unsigned int faceSize, float near = 0.1f, float far = 100.f);
55+
56+
/**
57+
* Performs a scan and returns hit points in world space.
58+
* The renderer's active render target is restored to nullptr after the scan.
59+
*/
60+
void scan(GLRenderer& renderer, Scene& scene, std::vector<Vector3>& cloud);
61+
62+
[[nodiscard]] unsigned int faceSize() const { return faceSize_; }
63+
[[nodiscard]] float near() const { return near_; }
64+
[[nodiscard]] float far() const { return far_; }
65+
66+
private:
67+
static constexpr int kNumFaces = 6;
68+
69+
unsigned int faceSize_;
70+
float near_;
71+
float far_;
72+
73+
Scene postScene_;
74+
OrthographicCamera postCamera_;
75+
std::shared_ptr<ShaderMaterial> postMaterial_;
76+
77+
// Non-owning pointers into the children list for fast per-face access
78+
std::array<PerspectiveCamera*, kNumFaces> cameras_{};
79+
std::array<std::unique_ptr<GLRenderTarget>, kNumFaces> sceneTargets_;
80+
std::array<std::unique_ptr<GLRenderTarget>, kNumFaces> readbackTargets_;
81+
82+
// Dense-grid mode: shared ray direction factors (tan(90°/2) = 1, so just NDC coords)
83+
std::vector<float> dir_;
84+
85+
// Model-based mode: one entry per beam
86+
struct BeamSample {
87+
uint8_t face;
88+
uint16_t pixelX, pixelY;
89+
float u, v;// exact NDC of this beam's direction in the face camera
90+
};
91+
std::vector<BeamSample> beams_;
92+
93+
void init(float near, float far);
94+
void buildBeamTable(const LidarModel& model);
95+
96+
void renderFaces(GLRenderer& renderer, Scene& scene);
97+
void unprojectDense(std::vector<Vector3>& points) const;
98+
void unprojectBeams(std::vector<Vector3>& points) const;
99+
};
100+
101+
}// namespace threepp
102+
103+
#endif//THREEPP_LIDARSENSOR_HPP

‎src/CMakeLists.txt‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -96,6 +96,8 @@ set(publicHeaders
9696
"threepp/helpers/BoxHelper.hpp"
9797
"threepp/helpers/CameraHelper.hpp"
9898
"threepp/helpers/DepthSensor.hpp"
99+
"threepp/helpers/LidarModel.hpp"
100+
"threepp/helpers/LidarSensor.hpp"
99101
"threepp/helpers/DirectionalLightHelper.hpp"
100102
"threepp/helpers/GridHelper.hpp"
101103
"threepp/helpers/HemisphereLightHelper.hpp"
@@ -332,6 +334,7 @@ set(sources
332334
"threepp/helpers/BoxHelper.cpp"
333335
"threepp/helpers/CameraHelper.cpp"
334336
"threepp/helpers/DepthSensor.cpp"
337+
"threepp/helpers/LidarSensor.cpp"
335338
"threepp/helpers/DirectionalLightHelper.cpp"
336339
"threepp/helpers/GridHelper.cpp"
337340
"threepp/helpers/HemisphereLightHelper.cpp"

0 commit comments

Comments
 (0)