Add depth sensing sample and OpenGL tests
- Implemented a new sample application in main.cpp to capture and display a live 3D point cloud using the ZED SDK and OpenGL. - Created test_opengl.cpp to demonstrate basic OpenGL rendering with depth testing using two triangles. - Added test_points.cpp to visualize a grid of colored points in OpenGL, showcasing point rendering capabilities.
This commit is contained in:
1
new_playground/depth-sensing/.gitignore
vendored
Normal file
1
new_playground/depth-sensing/.gitignore
vendored
Normal file
@ -0,0 +1 @@
|
|||||||
|
build
|
||||||
76
new_playground/depth-sensing/CMakeLists.txt
Executable file
76
new_playground/depth-sensing/CMakeLists.txt
Executable file
@ -0,0 +1,76 @@
|
|||||||
|
CMAKE_MINIMUM_REQUIRED(VERSION 3.5)
|
||||||
|
PROJECT(ZED_Depth_Sensing)
|
||||||
|
|
||||||
|
set(CMAKE_CXX_STANDARD 17)
|
||||||
|
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||||
|
SET(CMAKE_BUILD_TYPE "Release")
|
||||||
|
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
|
||||||
|
|
||||||
|
option(LINK_SHARED_ZED "Link with the ZED SDK shared executable" ON)
|
||||||
|
|
||||||
|
if (NOT LINK_SHARED_ZED AND MSVC)
|
||||||
|
message(FATAL_ERROR "LINK_SHARED_ZED OFF : ZED SDK static libraries not available on Windows")
|
||||||
|
endif()
|
||||||
|
|
||||||
|
find_package(ZED REQUIRED)
|
||||||
|
find_package(CUDA ${ZED_CUDA_VERSION} REQUIRED)
|
||||||
|
find_package(OpenCV REQUIRED)
|
||||||
|
find_package(GLUT REQUIRED)
|
||||||
|
find_package(GLEW REQUIRED)
|
||||||
|
SET(OpenGL_GL_PREFERENCE GLVND)
|
||||||
|
find_package(OpenGL REQUIRED)
|
||||||
|
|
||||||
|
include_directories(${ZED_INCLUDE_DIRS})
|
||||||
|
message(STATUS "ZED include dir: ${ZED_INCLUDE_DIRS}")
|
||||||
|
include_directories(${OpenCV_INCLUDE_DIRS})
|
||||||
|
include_directories(${GLEW_INCLUDE_DIRS})
|
||||||
|
include_directories(${GLUT_INCLUDE_DIR})
|
||||||
|
include_directories(${CUDA_INCLUDE_DIRS})
|
||||||
|
include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include)
|
||||||
|
|
||||||
|
link_directories(${ZED_LIBRARY_DIR})
|
||||||
|
link_directories(${GLEW_LIBRARY_DIRS})
|
||||||
|
link_directories(${GLUT_LIBRARY_DIRS})
|
||||||
|
link_directories(${OpenGL_LIBRARY_DIRS})
|
||||||
|
link_directories(${CUDA_LIBRARY_DIRS})
|
||||||
|
link_directories(${CMAKE_CURRENT_SOURCE_DIR}/lib)
|
||||||
|
|
||||||
|
IF (CMAKE_SYSTEM_PROCESSOR MATCHES aarch64)
|
||||||
|
add_definitions(-DJETSON_STYLE)
|
||||||
|
ENDIF()
|
||||||
|
|
||||||
|
FILE(GLOB_RECURSE SRC_FILES src/*.cpp)
|
||||||
|
FILE(GLOB_RECURSE HDR_FILES include/*.hpp)
|
||||||
|
|
||||||
|
ADD_EXECUTABLE(${PROJECT_NAME} ${HDR_FILES} ${SRC_FILES})
|
||||||
|
|
||||||
|
if (LINK_SHARED_ZED)
|
||||||
|
SET(ZED_LIBS ${ZED_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_CUDART_LIBRARY})
|
||||||
|
else()
|
||||||
|
SET(ZED_LIBS ${ZED_STATIC_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_LIBRARY})
|
||||||
|
endif()
|
||||||
|
|
||||||
|
TARGET_LINK_LIBRARIES(${PROJECT_NAME}
|
||||||
|
${ZED_LIBS}
|
||||||
|
${OPENGL_LIBRARIES}
|
||||||
|
${GLUT_LIBRARIES}
|
||||||
|
${GLEW_LIBRARIES}
|
||||||
|
${OpenCV_LIBRARIES})
|
||||||
|
|
||||||
|
# Test executables
|
||||||
|
ADD_EXECUTABLE(test_opengl test_opengl.cpp)
|
||||||
|
TARGET_LINK_LIBRARIES(test_opengl
|
||||||
|
${OPENGL_LIBRARIES}
|
||||||
|
${GLUT_LIBRARIES}
|
||||||
|
${GLEW_LIBRARIES})
|
||||||
|
|
||||||
|
ADD_EXECUTABLE(test_points test_points.cpp)
|
||||||
|
TARGET_LINK_LIBRARIES(test_points
|
||||||
|
${OPENGL_LIBRARIES}
|
||||||
|
${GLUT_LIBRARIES}
|
||||||
|
${GLEW_LIBRARIES})
|
||||||
|
|
||||||
|
if(INSTALL_SAMPLES)
|
||||||
|
LIST(APPEND SAMPLE_LIST ${PROJECT_NAME})
|
||||||
|
SET(SAMPLE_LIST "${SAMPLE_LIST}" PARENT_SCOPE)
|
||||||
|
endif()
|
||||||
179
new_playground/depth-sensing/FIXES_SUMMARY.md
Executable file
179
new_playground/depth-sensing/FIXES_SUMMARY.md
Executable file
@ -0,0 +1,179 @@
|
|||||||
|
# ZED Point Cloud Rendering - Fix Summary
|
||||||
|
|
||||||
|
## Problem
|
||||||
|
The ZED depth sensing application was displaying RGB and depth images correctly via OpenCV, but the 3D point cloud viewer (GLViewer) showed nothing except the camera frustum wireframe. Point cloud data was valid (confirmed via PLY file export), but invisible in the OpenGL window.
|
||||||
|
|
||||||
|
## Root Causes
|
||||||
|
|
||||||
|
### 1. **Empty OpenGL VBO** (Critical Issue)
|
||||||
|
- **Problem**: The OpenGL Vertex Buffer Object (VBO) contained all zeros despite valid point cloud data from the ZED camera
|
||||||
|
- **Cause**: CUDA-OpenGL interop buffer was mapped once at initialization and never unmapped, preventing proper data transfer
|
||||||
|
- **Fix**: Changed `PointCloud::update()` to properly:
|
||||||
|
- Map the CUDA-OpenGL resource
|
||||||
|
- Copy data from ZED GPU buffer to mapped OpenGL buffer
|
||||||
|
- **Synchronize the CUDA stream** (`cudaStreamSynchronize`) - critical for ensuring copy completes
|
||||||
|
- Unmap the resource so OpenGL can access it
|
||||||
|
|
||||||
|
```cpp
|
||||||
|
// Before: Buffer stayed mapped, copy never completed properly
|
||||||
|
cudaGraphicsMapResources(1, &bufferCudaID_, 0); // At init only
|
||||||
|
cudaMemcpyAsync(...); // Copy happened but never synced
|
||||||
|
|
||||||
|
// After: Proper map → copy → sync → unmap cycle
|
||||||
|
cudaGraphicsMapResources(1, &bufferCudaID_, strm);
|
||||||
|
cudaMemcpyAsync(...);
|
||||||
|
cudaStreamSynchronize(strm); // Wait for GPU copy to complete!
|
||||||
|
cudaGraphicsUnmapResources(1, &bufferCudaID_, strm);
|
||||||
|
```
|
||||||
|
|
||||||
|
### 2. **Coordinate System Mismatch** (Critical Issue)
|
||||||
|
- **Problem**: Point cloud data is in millimeters (-2500 to +2500 mm), but camera and projection matrix expected smaller values
|
||||||
|
- **Cause**: ZED SDK outputs coordinates in millimeters, typical OpenGL coordinates are -1.0 to 1.0 or meters
|
||||||
|
- **Fix**: Added scaling in the vertex shader to convert mm → meters (÷1000)
|
||||||
|
|
||||||
|
```glsl
|
||||||
|
// Before: Raw millimeter coordinates
|
||||||
|
gl_Position = u_mvpMatrix * vec4(in_VertexRGBA.xyz, 1);
|
||||||
|
|
||||||
|
// After: Convert to meters in shader
|
||||||
|
vec3 pos_meters = in_VertexRGBA.xyz * 0.001; // mm to m
|
||||||
|
gl_Position = u_mvpMatrix * vec4(pos_meters, 1);
|
||||||
|
```
|
||||||
|
|
||||||
|
### 3. **Camera Positioning in Wrong Scale**
|
||||||
|
- **Problem**: Camera positioned at (0, 0, 2000) looking at (0, 0, -3500) - these are millimeters, but after shader scaling, points are in meters
|
||||||
|
- **Cause**: Camera transform wasn't updated to match the scaled point cloud
|
||||||
|
- **Fix**: Changed camera to use meters
|
||||||
|
|
||||||
|
```cpp
|
||||||
|
// Before: Millimeters (mismatched with scaled points)
|
||||||
|
camera_ = CameraGL(sl::Translation(0, 0, 2000), sl::Translation(0, 0, -3500));
|
||||||
|
setProjection(90, 90, 10.f, 50000.f); // znear=10mm, zfar=50000mm
|
||||||
|
|
||||||
|
// After: Meters (matches shader-scaled points)
|
||||||
|
camera_ = CameraGL(sl::Translation(0, 0, 2), sl::Translation(0, 0, -3.5));
|
||||||
|
setProjection(90, 90, 0.01f, 50.f); // znear=0.01m, zfar=50m
|
||||||
|
```
|
||||||
|
|
||||||
|
### 4. **Matrix Transpose Inconsistency** (Minor Issue)
|
||||||
|
- **Problem**: Frustum rendering used `GL_FALSE` with explicit `transpose()`, but point cloud used `GL_TRUE` without transpose
|
||||||
|
- **Fix**: Made both use `GL_FALSE` with explicit transpose for consistency
|
||||||
|
|
||||||
|
```cpp
|
||||||
|
// Point cloud now matches frustum:
|
||||||
|
glUniformMatrix4fv(shMVPMatrixLoc_, 1, GL_FALSE, sl::Transform::transpose(vp).m);
|
||||||
|
```
|
||||||
|
|
||||||
|
## Changes Made
|
||||||
|
|
||||||
|
### `src/GLViewer.cpp`
|
||||||
|
|
||||||
|
#### PointCloud::initialize()
|
||||||
|
- Changed CUDA buffer registration flag from `cudaGraphicsRegisterFlagsNone` to `cudaGraphicsRegisterFlagsWriteDiscard`
|
||||||
|
- Removed permanent buffer mapping at initialization
|
||||||
|
- Set `numBytes_` directly instead of mapping to get it
|
||||||
|
|
||||||
|
#### PointCloud::update()
|
||||||
|
- Implemented proper CUDA-OpenGL synchronization cycle:
|
||||||
|
- Map resource for CUDA access
|
||||||
|
- Copy data with `cudaMemcpyAsync`
|
||||||
|
- **Added `cudaStreamSynchronize(strm)`** - ensures GPU copy completes before unmapping
|
||||||
|
- Unmap resource for OpenGL rendering
|
||||||
|
|
||||||
|
#### PointCloud::draw()
|
||||||
|
- Simplified rendering (removed debug code)
|
||||||
|
- Kept depth test disabled for point cloud
|
||||||
|
- Set point size to 2.0 pixels
|
||||||
|
|
||||||
|
#### PointCloud Vertex Shader
|
||||||
|
- Added coordinate scaling: `vec3 pos_meters = in_VertexRGBA.xyz * 0.001;`
|
||||||
|
- This converts millimeter coordinates from ZED SDK to meters for OpenGL
|
||||||
|
|
||||||
|
#### GLViewer::init()
|
||||||
|
- Updated camera positioning from millimeters to meters:
|
||||||
|
- Position: `(0, 0, 2)` meters (was 2000mm)
|
||||||
|
- Look-at: `(0, 0, -3.5)` meters (was -3500mm)
|
||||||
|
- Updated projection clipping planes:
|
||||||
|
- Near: `0.01m` (10mm, was 10.f which was ambiguous)
|
||||||
|
- Far: `50m` (50,000mm, was 50000.f)
|
||||||
|
|
||||||
|
#### GLViewer::update()
|
||||||
|
- Updated mouse translation and zoom to use meters instead of millimeters
|
||||||
|
- Removed `* 1000` multipliers from camera movement
|
||||||
|
|
||||||
|
## Test Files Created
|
||||||
|
|
||||||
|
### `test_opengl.cpp`
|
||||||
|
- Minimal OpenGL test with triangles
|
||||||
|
- Used to verify basic OpenGL/X11 rendering worked
|
||||||
|
- Helped isolate that depth buffer was working
|
||||||
|
|
||||||
|
### `test_points.cpp`
|
||||||
|
- Minimal point cloud rendering test
|
||||||
|
- Confirmed GL_POINTS rendering worked in X11 forwarding
|
||||||
|
- Used simple coordinates (-0.8 to 0.8) that proved the transformation was the issue
|
||||||
|
|
||||||
|
### `CMakeLists.txt`
|
||||||
|
- Added test executables for isolated OpenGL testing
|
||||||
|
|
||||||
|
## Key Insights
|
||||||
|
|
||||||
|
1. **CUDA Stream Synchronization is Critical**: Without `cudaStreamSynchronize()`, the async GPU copy may not complete before OpenGL tries to render, resulting in empty/stale data
|
||||||
|
|
||||||
|
2. **Coordinate Scale Matters**: OpenGL projection matrices work best with reasonable coordinate ranges (meters, not millimeters). The 1000x difference was causing precision issues and made debugging harder
|
||||||
|
|
||||||
|
3. **X11 Forwarding Works**: Despite initial concerns about X11 forwarding limitations in dev containers, OpenGL 4.6 with NVIDIA hardware acceleration works fine - the issue was entirely in our code
|
||||||
|
|
||||||
|
4. **Data vs Rendering Separation**: The point cloud data was always valid (confirmed by PLY export and Open3D visualization). The problem was purely in the OpenGL rendering pipeline
|
||||||
|
|
||||||
|
5. **Debugging with VBO Readback**: Using `glGetBufferSubData()` to read back GPU buffer contents was crucial for discovering the VBO was empty
|
||||||
|
|
||||||
|
## Current Status
|
||||||
|
|
||||||
|
✅ **Working Features:**
|
||||||
|
- RGB camera view display (OpenCV window)
|
||||||
|
- Depth image display (OpenCV window)
|
||||||
|
- 3D point cloud visualization (OpenGL window)
|
||||||
|
- Real-time camera data streaming
|
||||||
|
- Debug mode with synthetic rainbow point cloud
|
||||||
|
- Mouse controls for camera movement (rotate, pan, zoom)
|
||||||
|
- PLY file export
|
||||||
|
|
||||||
|
✅ **Performance:**
|
||||||
|
- 30 FPS with 491,520 points (960x512 resolution)
|
||||||
|
- GPU-accelerated rendering via CUDA-OpenGL interop
|
||||||
|
- No visible lag or stuttering
|
||||||
|
|
||||||
|
## Usage
|
||||||
|
|
||||||
|
```bash
|
||||||
|
# Real camera data
|
||||||
|
./ZED_Depth_Sensing
|
||||||
|
|
||||||
|
# Synthetic test data (rainbow gradient)
|
||||||
|
./ZED_Depth_Sensing --debug
|
||||||
|
```
|
||||||
|
|
||||||
|
**Controls:**
|
||||||
|
- Left mouse: Rotate camera
|
||||||
|
- Right mouse: Pan camera
|
||||||
|
- Mouse wheel: Zoom in/out
|
||||||
|
- 's' key: Save current point cloud to .ply file
|
||||||
|
- ESC: Exit
|
||||||
|
|
||||||
|
## Files Modified
|
||||||
|
|
||||||
|
- `src/GLViewer.cpp` - Main rendering and CUDA-OpenGL interop fixes
|
||||||
|
- `src/main.cpp` - Already had RGB/depth capture and debug mode
|
||||||
|
- `CMakeLists.txt` - Added test executables
|
||||||
|
- `test_opengl.cpp` - Created for testing
|
||||||
|
- `test_points.cpp` - Created for testing
|
||||||
|
|
||||||
|
## Technical Details
|
||||||
|
|
||||||
|
**Hardware:** NVIDIA GeForce RTX 5070 Ti
|
||||||
|
**OpenGL:** 4.6.0 NVIDIA 590.48.01
|
||||||
|
**GLSL:** 4.60 NVIDIA
|
||||||
|
**ZED SDK:** 4.x with NEURAL_PLUS depth mode
|
||||||
|
**Resolution:** HD720 (960x512)
|
||||||
|
**Environment:** Dev container with X11 forwarding
|
||||||
24
new_playground/depth-sensing/README.md
Executable file
24
new_playground/depth-sensing/README.md
Executable file
@ -0,0 +1,24 @@
|
|||||||
|
# ZED SDK - Depth Sensing
|
||||||
|
|
||||||
|
This sample shows how to retreive the current point cloud.
|
||||||
|
|
||||||
|
## Getting Started
|
||||||
|
- Get the latest [ZED SDK](https://www.stereolabs.com/developers/release/)
|
||||||
|
- Check the [Documentation](https://www.stereolabs.com/docs/)
|
||||||
|
|
||||||
|
## Build the program
|
||||||
|
- Build for [Windows](https://www.stereolabs.com/docs/app-development/cpp/windows/)
|
||||||
|
- Build for [Linux/Jetson](https://www.stereolabs.com/docs/app-development/cpp/linux/)
|
||||||
|
|
||||||
|
## Run the program
|
||||||
|
- Navigate to the build directory and launch the executable
|
||||||
|
- Or open a terminal in the build directory and run the sample :
|
||||||
|
|
||||||
|
./ZED_Depth_Sensing
|
||||||
|
|
||||||
|
### Features
|
||||||
|
- Camera live point cloud is retreived
|
||||||
|
- An OpenGL windows displays it in 3D
|
||||||
|
|
||||||
|
## Support
|
||||||
|
If you need assistance go to our Community site at https://community.stereolabs.com/
|
||||||
273
new_playground/depth-sensing/include/GLViewer.hpp
Executable file
273
new_playground/depth-sensing/include/GLViewer.hpp
Executable file
@ -0,0 +1,273 @@
|
|||||||
|
#ifndef __VIEWER_INCLUDE__
|
||||||
|
#define __VIEWER_INCLUDE__
|
||||||
|
|
||||||
|
#include <vector>
|
||||||
|
#include <mutex>
|
||||||
|
|
||||||
|
#include <sl/Camera.hpp>
|
||||||
|
|
||||||
|
#include <GL/glew.h>
|
||||||
|
#include <GL/freeglut.h>
|
||||||
|
|
||||||
|
#include <cuda.h>
|
||||||
|
#include <cuda_gl_interop.h>
|
||||||
|
|
||||||
|
#ifndef M_PI
|
||||||
|
#define M_PI 3.141592653f
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#define MOUSE_R_SENSITIVITY 0.03f
|
||||||
|
#define MOUSE_UZ_SENSITIVITY 0.5f
|
||||||
|
#define MOUSE_DZ_SENSITIVITY 1.25f
|
||||||
|
#define MOUSE_T_SENSITIVITY 0.05f
|
||||||
|
#define KEY_T_SENSITIVITY 0.1f
|
||||||
|
|
||||||
|
|
||||||
|
//// UTILS //////
|
||||||
|
using namespace std;
|
||||||
|
void print(std::string msg_prefix, sl::ERROR_CODE err_code = sl::ERROR_CODE::SUCCESS, std::string msg_suffix = "") ;
|
||||||
|
|
||||||
|
/////////////////
|
||||||
|
|
||||||
|
class CameraGL {
|
||||||
|
public:
|
||||||
|
|
||||||
|
CameraGL() {}
|
||||||
|
enum DIRECTION {
|
||||||
|
UP, DOWN, LEFT, RIGHT, FORWARD, BACK
|
||||||
|
};
|
||||||
|
CameraGL(sl::Translation position, sl::Translation direction, sl::Translation vertical = sl::Translation(0, 1, 0)); // vertical = Eigen::Vector3f(0, 1, 0)
|
||||||
|
~CameraGL();
|
||||||
|
|
||||||
|
void update();
|
||||||
|
void setProjection(float horizontalFOV, float verticalFOV, float znear, float zfar);
|
||||||
|
const sl::Transform& getViewProjectionMatrix() const;
|
||||||
|
|
||||||
|
float getHorizontalFOV() const;
|
||||||
|
float getVerticalFOV() const;
|
||||||
|
|
||||||
|
// Set an offset between the eye of the camera and its position
|
||||||
|
// Note: Useful to use the camera as a trackball camera with z>0 and x = 0, y = 0
|
||||||
|
// Note: coordinates are in local space
|
||||||
|
void setOffsetFromPosition(const sl::Translation& offset);
|
||||||
|
const sl::Translation& getOffsetFromPosition() const;
|
||||||
|
|
||||||
|
void setDirection(const sl::Translation& direction, const sl::Translation &vertical);
|
||||||
|
void translate(const sl::Translation& t);
|
||||||
|
void setPosition(const sl::Translation& p);
|
||||||
|
void rotate(const sl::Orientation& rot);
|
||||||
|
void rotate(const sl::Rotation& m);
|
||||||
|
void setRotation(const sl::Orientation& rot);
|
||||||
|
void setRotation(const sl::Rotation& m);
|
||||||
|
|
||||||
|
const sl::Translation& getPosition() const;
|
||||||
|
const sl::Translation& getForward() const;
|
||||||
|
const sl::Translation& getRight() const;
|
||||||
|
const sl::Translation& getUp() const;
|
||||||
|
const sl::Translation& getVertical() const;
|
||||||
|
float getZNear() const;
|
||||||
|
float getZFar() const;
|
||||||
|
|
||||||
|
static const sl::Translation ORIGINAL_FORWARD;
|
||||||
|
static const sl::Translation ORIGINAL_UP;
|
||||||
|
static const sl::Translation ORIGINAL_RIGHT;
|
||||||
|
|
||||||
|
sl::Transform projection_;
|
||||||
|
private:
|
||||||
|
void updateVectors();
|
||||||
|
void updateView();
|
||||||
|
void updateVPMatrix();
|
||||||
|
|
||||||
|
sl::Translation offset_;
|
||||||
|
sl::Translation position_;
|
||||||
|
sl::Translation forward_;
|
||||||
|
sl::Translation up_;
|
||||||
|
sl::Translation right_;
|
||||||
|
sl::Translation vertical_;
|
||||||
|
|
||||||
|
sl::Orientation rotation_;
|
||||||
|
|
||||||
|
sl::Transform view_;
|
||||||
|
sl::Transform vpMatrix_;
|
||||||
|
float horizontalFieldOfView_;
|
||||||
|
float verticalFieldOfView_;
|
||||||
|
float znear_;
|
||||||
|
float zfar_;
|
||||||
|
};
|
||||||
|
|
||||||
|
class Shader {
|
||||||
|
public:
|
||||||
|
|
||||||
|
Shader() : verterxId_(0), fragmentId_(0), programId_(0) {}
|
||||||
|
Shader(const GLchar* vs, const GLchar* fs);
|
||||||
|
~Shader();
|
||||||
|
|
||||||
|
// Delete the move constructor and move assignment operator
|
||||||
|
Shader(Shader&&) = delete;
|
||||||
|
Shader& operator=(Shader&&) = delete;
|
||||||
|
|
||||||
|
// Delete the copy constructor and copy assignment operator
|
||||||
|
Shader(const Shader&) = delete;
|
||||||
|
Shader& operator=(const Shader&) = delete;
|
||||||
|
|
||||||
|
void set(const GLchar* vs, const GLchar* fs);
|
||||||
|
GLuint getProgramId();
|
||||||
|
|
||||||
|
static const GLint ATTRIB_VERTICES_POS = 0;
|
||||||
|
static const GLint ATTRIB_COLOR_POS = 1;
|
||||||
|
private:
|
||||||
|
bool compile(GLuint &shaderId, GLenum type, const GLchar* src);
|
||||||
|
GLuint verterxId_;
|
||||||
|
GLuint fragmentId_;
|
||||||
|
GLuint programId_;
|
||||||
|
};
|
||||||
|
|
||||||
|
class Simple3DObject {
|
||||||
|
public:
|
||||||
|
|
||||||
|
Simple3DObject();
|
||||||
|
Simple3DObject(sl::Translation position, bool isStatic);
|
||||||
|
~Simple3DObject();
|
||||||
|
|
||||||
|
void addPoint(sl::float3 pt, sl::float3 clr);
|
||||||
|
void addFace(sl::float3 p1, sl::float3 p2, sl::float3 p3, sl::float3 clr);
|
||||||
|
void pushToGPU();
|
||||||
|
void clear();
|
||||||
|
|
||||||
|
void setDrawingType(GLenum type);
|
||||||
|
|
||||||
|
void draw();
|
||||||
|
|
||||||
|
void translate(const sl::Translation& t);
|
||||||
|
void setPosition(const sl::Translation& p);
|
||||||
|
|
||||||
|
void setRT(const sl::Transform& mRT);
|
||||||
|
|
||||||
|
void rotate(const sl::Orientation& rot);
|
||||||
|
void rotate(const sl::Rotation& m);
|
||||||
|
void setRotation(const sl::Orientation& rot);
|
||||||
|
void setRotation(const sl::Rotation& m);
|
||||||
|
|
||||||
|
const sl::Translation& getPosition() const;
|
||||||
|
|
||||||
|
sl::Transform getModelMatrix() const;
|
||||||
|
private:
|
||||||
|
std::vector<float> vertices_;
|
||||||
|
std::vector<float> colors_;
|
||||||
|
std::vector<unsigned int> indices_;
|
||||||
|
|
||||||
|
bool isStatic_;
|
||||||
|
|
||||||
|
GLenum drawingType_;
|
||||||
|
|
||||||
|
GLuint vaoID_;
|
||||||
|
/*
|
||||||
|
Vertex buffer IDs:
|
||||||
|
- [0]: Vertices coordinates;
|
||||||
|
- [1]: Vertices colors;
|
||||||
|
- [2]: Indices;
|
||||||
|
*/
|
||||||
|
GLuint vboID_[3];
|
||||||
|
|
||||||
|
sl::Translation position_;
|
||||||
|
sl::Orientation rotation_;
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
class PointCloud {
|
||||||
|
public:
|
||||||
|
PointCloud();
|
||||||
|
~PointCloud();
|
||||||
|
|
||||||
|
// Initialize Opengl and Cuda buffers
|
||||||
|
// Warning: must be called in the Opengl thread
|
||||||
|
void initialize(sl::Resolution res, CUstream strm);
|
||||||
|
// Push a new point cloud
|
||||||
|
// Warning: can be called from any thread but the mutex "mutexData" must be locked
|
||||||
|
void pushNewPC(sl::Mat &matXYZRGBA);
|
||||||
|
// Update the Opengl buffer
|
||||||
|
// Warning: must be called in the Opengl thread
|
||||||
|
void update();
|
||||||
|
// Draw the point cloud
|
||||||
|
// Warning: must be called in the Opengl thread
|
||||||
|
void draw(const sl::Transform& vp);
|
||||||
|
// Close (disable update)
|
||||||
|
void close();
|
||||||
|
|
||||||
|
std::mutex mutexData;
|
||||||
|
private:
|
||||||
|
sl::Mat matGPU_;
|
||||||
|
bool hasNewPCL_ = false;
|
||||||
|
Shader shader_;
|
||||||
|
GLuint shMVPMatrixLoc_;
|
||||||
|
size_t numBytes_;
|
||||||
|
float* xyzrgbaMappedBuf_;
|
||||||
|
GLuint bufferGLID_;
|
||||||
|
cudaGraphicsResource* bufferCudaID_;
|
||||||
|
CUstream strm;
|
||||||
|
};
|
||||||
|
|
||||||
|
// This class manages input events, window and Opengl rendering pipeline
|
||||||
|
class GLViewer {
|
||||||
|
public:
|
||||||
|
GLViewer();
|
||||||
|
~GLViewer();
|
||||||
|
bool isAvailable();
|
||||||
|
|
||||||
|
GLenum init(int argc, char **argv, sl::CameraParameters param, CUstream strm, sl::Resolution image_size);
|
||||||
|
void updatePointCloud(sl::Mat &matXYZRGBA);
|
||||||
|
|
||||||
|
void exit();
|
||||||
|
bool shouldSaveData();
|
||||||
|
private:
|
||||||
|
// Rendering loop method called each frame by glutDisplayFunc
|
||||||
|
void render();
|
||||||
|
// Everything that needs to be updated before rendering must be done in this method
|
||||||
|
void update();
|
||||||
|
// Once everything is updated, every renderable objects must be drawn in this method
|
||||||
|
void draw();
|
||||||
|
// Clear and refresh inputs' data
|
||||||
|
void clearInputs();
|
||||||
|
|
||||||
|
// Glut functions callbacks
|
||||||
|
static void drawCallback();
|
||||||
|
static void mouseButtonCallback(int button, int state, int x, int y);
|
||||||
|
static void mouseMotionCallback(int x, int y);
|
||||||
|
static void reshapeCallback(int width, int height);
|
||||||
|
static void keyPressedCallback(unsigned char c, int x, int y);
|
||||||
|
static void keyReleasedCallback(unsigned char c, int x, int y);
|
||||||
|
static void idle();
|
||||||
|
|
||||||
|
bool available;
|
||||||
|
|
||||||
|
enum MOUSE_BUTTON {
|
||||||
|
LEFT = 0,
|
||||||
|
MIDDLE = 1,
|
||||||
|
RIGHT = 2,
|
||||||
|
WHEEL_UP = 3,
|
||||||
|
WHEEL_DOWN = 4
|
||||||
|
};
|
||||||
|
|
||||||
|
enum KEY_STATE {
|
||||||
|
UP = 'u',
|
||||||
|
DOWN = 'd',
|
||||||
|
FREE = 'f'
|
||||||
|
};
|
||||||
|
|
||||||
|
bool mouseButton_[3];
|
||||||
|
int mouseWheelPosition_;
|
||||||
|
int mouseCurrentPosition_[2];
|
||||||
|
int mouseMotion_[2];
|
||||||
|
int previousMouseMotion_[2];
|
||||||
|
KEY_STATE keyStates_[256];
|
||||||
|
sl::float3 bckgrnd_clr;
|
||||||
|
|
||||||
|
Simple3DObject frustum;
|
||||||
|
PointCloud pointCloud_;
|
||||||
|
CameraGL camera_;
|
||||||
|
Shader shader_;
|
||||||
|
GLuint shMVPMatrixLoc_;
|
||||||
|
bool shouldSaveData_ = false;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif /* __VIEWER_INCLUDE__ */
|
||||||
778
new_playground/depth-sensing/src/GLViewer.cpp
Executable file
778
new_playground/depth-sensing/src/GLViewer.cpp
Executable file
@ -0,0 +1,778 @@
|
|||||||
|
#include "GLViewer.hpp"
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
void print(std::string msg_prefix, sl::ERROR_CODE err_code, std::string msg_suffix) {
|
||||||
|
cout <<"[Sample]";
|
||||||
|
if (err_code != sl::ERROR_CODE::SUCCESS)
|
||||||
|
cout << "[Error] ";
|
||||||
|
else
|
||||||
|
cout<<" ";
|
||||||
|
cout << msg_prefix << " ";
|
||||||
|
if (err_code != sl::ERROR_CODE::SUCCESS) {
|
||||||
|
cout << " | " << toString(err_code) << " : ";
|
||||||
|
cout << toVerbose(err_code);
|
||||||
|
}
|
||||||
|
if (!msg_suffix.empty())
|
||||||
|
cout << " " << msg_suffix;
|
||||||
|
cout << endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
const GLchar* VERTEX_SHADER =
|
||||||
|
"#version 330 core\n"
|
||||||
|
"layout(location = 0) in vec3 in_Vertex;\n"
|
||||||
|
"layout(location = 1) in vec3 in_Color;\n"
|
||||||
|
"uniform mat4 u_mvpMatrix;\n"
|
||||||
|
"out vec3 b_color;\n"
|
||||||
|
"void main() {\n"
|
||||||
|
" b_color = in_Color;\n"
|
||||||
|
" gl_Position = u_mvpMatrix * vec4(in_Vertex, 1);\n"
|
||||||
|
"}";
|
||||||
|
|
||||||
|
const GLchar* FRAGMENT_SHADER =
|
||||||
|
"#version 330 core\n"
|
||||||
|
"in vec3 b_color;\n"
|
||||||
|
"layout(location = 0) out vec4 out_Color;\n"
|
||||||
|
"void main() {\n"
|
||||||
|
" out_Color = vec4(b_color, 1);\n"
|
||||||
|
"}";
|
||||||
|
|
||||||
|
GLViewer* currentInstance_ = nullptr;
|
||||||
|
|
||||||
|
GLViewer::GLViewer() : available(false){
|
||||||
|
currentInstance_ = this;
|
||||||
|
mouseButton_[0] = mouseButton_[1] = mouseButton_[2] = false;
|
||||||
|
clearInputs();
|
||||||
|
previousMouseMotion_[0] = previousMouseMotion_[1] = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
GLViewer::~GLViewer() {}
|
||||||
|
|
||||||
|
void GLViewer::exit() {
|
||||||
|
if (currentInstance_) {
|
||||||
|
//pointCloud_.close();
|
||||||
|
available = false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool GLViewer::isAvailable() {
|
||||||
|
if(available)
|
||||||
|
glutMainLoopEvent();
|
||||||
|
return available;
|
||||||
|
}
|
||||||
|
|
||||||
|
Simple3DObject createFrustum(sl::CameraParameters param) {
|
||||||
|
|
||||||
|
// Create 3D axis
|
||||||
|
Simple3DObject it(sl::Translation(0, 0, 0), true);
|
||||||
|
|
||||||
|
float Z_ = -150;
|
||||||
|
sl::float3 cam_0(0, 0, 0);
|
||||||
|
sl::float3 cam_1, cam_2, cam_3, cam_4;
|
||||||
|
|
||||||
|
float fx_ = 1.f / param.fx;
|
||||||
|
float fy_ = 1.f / param.fy;
|
||||||
|
|
||||||
|
cam_1.z = Z_;
|
||||||
|
cam_1.x = (0 - param.cx) * Z_ *fx_;
|
||||||
|
cam_1.y = (0 - param.cy) * Z_ *fy_;
|
||||||
|
|
||||||
|
cam_2.z = Z_;
|
||||||
|
cam_2.x = (param.image_size.width - param.cx) * Z_ *fx_;
|
||||||
|
cam_2.y = (0 - param.cy) * Z_ *fy_;
|
||||||
|
|
||||||
|
cam_3.z = Z_;
|
||||||
|
cam_3.x = (param.image_size.width - param.cx) * Z_ *fx_;
|
||||||
|
cam_3.y = (param.image_size.height - param.cy) * Z_ *fy_;
|
||||||
|
|
||||||
|
cam_4.z = Z_;
|
||||||
|
cam_4.x = (0 - param.cx) * Z_ *fx_;
|
||||||
|
cam_4.y = (param.image_size.height - param.cy) * Z_ *fy_;
|
||||||
|
|
||||||
|
float const to_f = 1.f/ 255.f;
|
||||||
|
const sl::float4 clr_lime(217*to_f,255*to_f,66*to_f, 1.f);
|
||||||
|
|
||||||
|
it.addPoint(cam_0, clr_lime);
|
||||||
|
it.addPoint(cam_1, clr_lime);
|
||||||
|
|
||||||
|
it.addPoint(cam_0, clr_lime);
|
||||||
|
it.addPoint(cam_2, clr_lime);
|
||||||
|
|
||||||
|
it.addPoint(cam_0, clr_lime);
|
||||||
|
it.addPoint(cam_3, clr_lime);
|
||||||
|
|
||||||
|
it.addPoint(cam_0, clr_lime);
|
||||||
|
it.addPoint(cam_4, clr_lime);
|
||||||
|
|
||||||
|
|
||||||
|
it.setDrawingType(GL_LINES);
|
||||||
|
return it;
|
||||||
|
}
|
||||||
|
|
||||||
|
void CloseFunc(void) { if(currentInstance_) currentInstance_->exit();}
|
||||||
|
|
||||||
|
GLenum GLViewer::init(int argc, char **argv, sl::CameraParameters param, CUstream strm_, sl::Resolution image_size) {
|
||||||
|
|
||||||
|
glutInit(&argc, argv);
|
||||||
|
int wnd_w = glutGet(GLUT_SCREEN_WIDTH);
|
||||||
|
int wnd_h = glutGet(GLUT_SCREEN_HEIGHT) *0.9;
|
||||||
|
glutInitWindowSize(wnd_w*0.9, wnd_h*0.9);
|
||||||
|
glutInitWindowPosition(wnd_w*0.05, wnd_h*0.05);
|
||||||
|
glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH);
|
||||||
|
glutCreateWindow("ZED Depth Sensing");
|
||||||
|
|
||||||
|
GLenum err = glewInit();
|
||||||
|
if (GLEW_OK != err)
|
||||||
|
return err;
|
||||||
|
|
||||||
|
// Print OpenGL context info
|
||||||
|
std::cout << "\n=== OpenGL Context Information ===" << std::endl;
|
||||||
|
std::cout << "GL Vendor: " << glGetString(GL_VENDOR) << std::endl;
|
||||||
|
std::cout << "GL Renderer: " << glGetString(GL_RENDERER) << std::endl;
|
||||||
|
std::cout << "GL Version: " << glGetString(GL_VERSION) << std::endl;
|
||||||
|
std::cout << "GLSL Version: " << glGetString(GL_SHADING_LANGUAGE_VERSION) << std::endl;
|
||||||
|
std::cout << "==================================\n" << std::endl;
|
||||||
|
|
||||||
|
pointCloud_.initialize(image_size, strm_);
|
||||||
|
|
||||||
|
// Compile and create the shader
|
||||||
|
shader_.set(VERTEX_SHADER, FRAGMENT_SHADER);
|
||||||
|
shMVPMatrixLoc_ = glGetUniformLocation(shader_.getProgramId(), "u_mvpMatrix");
|
||||||
|
|
||||||
|
// Create the camera - point cloud center is around Z=-3 to -4 meters (after shader scaling)
|
||||||
|
// Place camera at 2 meters looking at -3.5 meters (now in meters to match shader scaling)
|
||||||
|
camera_ = CameraGL(sl::Translation(0, 0, 2), sl::Translation(0, 0, -3.5));
|
||||||
|
|
||||||
|
sl::Rotation rot;
|
||||||
|
rot.setEulerAngles(sl::float3(0,0,0), false);
|
||||||
|
camera_.setRotation(rot);
|
||||||
|
|
||||||
|
frustum = createFrustum(param);
|
||||||
|
frustum.pushToGPU();
|
||||||
|
|
||||||
|
bckgrnd_clr = sl::float3(59, 63, 69);
|
||||||
|
bckgrnd_clr /= 255.f;
|
||||||
|
|
||||||
|
// Map glut function on this class methods
|
||||||
|
glutDisplayFunc(GLViewer::drawCallback);
|
||||||
|
glutMouseFunc(GLViewer::mouseButtonCallback);
|
||||||
|
glutMotionFunc(GLViewer::mouseMotionCallback);
|
||||||
|
glutReshapeFunc(GLViewer::reshapeCallback);
|
||||||
|
glutKeyboardFunc(GLViewer::keyPressedCallback);
|
||||||
|
glutKeyboardUpFunc(GLViewer::keyReleasedCallback);
|
||||||
|
glutCloseFunc(CloseFunc);
|
||||||
|
|
||||||
|
|
||||||
|
glEnable(GL_DEPTH_TEST);
|
||||||
|
#ifndef JETSON_STYLE
|
||||||
|
glEnable(GL_LINE_SMOOTH);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
available = true;
|
||||||
|
return err;
|
||||||
|
}
|
||||||
|
|
||||||
|
void GLViewer::render() {
|
||||||
|
if (available) {
|
||||||
|
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
|
||||||
|
glClearColor(bckgrnd_clr.r, bckgrnd_clr.g, bckgrnd_clr.b, 1.f);
|
||||||
|
glPointSize(2.f); // Reset to normal size - was affecting frustum
|
||||||
|
update();
|
||||||
|
draw();
|
||||||
|
glutSwapBuffers();
|
||||||
|
glutPostRedisplay();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void GLViewer::updatePointCloud(sl::Mat &matXYZRGBA) {
|
||||||
|
pointCloud_.mutexData.lock();
|
||||||
|
pointCloud_.pushNewPC(matXYZRGBA);
|
||||||
|
pointCloud_.mutexData.unlock();
|
||||||
|
}
|
||||||
|
|
||||||
|
bool GLViewer::shouldSaveData(){
|
||||||
|
bool out = shouldSaveData_;
|
||||||
|
shouldSaveData_ = false;
|
||||||
|
return out;
|
||||||
|
}
|
||||||
|
|
||||||
|
void GLViewer::update() {
|
||||||
|
if (keyStates_['q'] == KEY_STATE::UP || keyStates_['Q'] == KEY_STATE::UP || keyStates_[27] == KEY_STATE::UP) {
|
||||||
|
pointCloud_.close();
|
||||||
|
currentInstance_->exit();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if(keyStates_['s'] == KEY_STATE::UP || keyStates_['s'] == KEY_STATE::UP)
|
||||||
|
currentInstance_->shouldSaveData_ = true;
|
||||||
|
|
||||||
|
// Rotate camera with mouse
|
||||||
|
if (mouseButton_[MOUSE_BUTTON::LEFT]) {
|
||||||
|
camera_.rotate(sl::Rotation((float)mouseMotion_[1] * MOUSE_R_SENSITIVITY, camera_.getRight()));
|
||||||
|
camera_.rotate(sl::Rotation((float)mouseMotion_[0] * MOUSE_R_SENSITIVITY, camera_.getVertical() * -1.f));
|
||||||
|
}
|
||||||
|
|
||||||
|
// Translate camera with mouse (now using meters)
|
||||||
|
if (mouseButton_[MOUSE_BUTTON::RIGHT]) {
|
||||||
|
camera_.translate(camera_.getUp() * (float)mouseMotion_[1] * MOUSE_T_SENSITIVITY);
|
||||||
|
camera_.translate(camera_.getRight() * (float)mouseMotion_[0] * MOUSE_T_SENSITIVITY);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Zoom in with mouse wheel (now using meters)
|
||||||
|
if (mouseWheelPosition_ != 0) {
|
||||||
|
if (mouseWheelPosition_ > 0) { // zoom
|
||||||
|
camera_.translate(camera_.getForward() * MOUSE_UZ_SENSITIVITY * -1);
|
||||||
|
}
|
||||||
|
else if (mouseWheelPosition_ < 0) {// unzoom
|
||||||
|
camera_.translate(camera_.getForward() * MOUSE_UZ_SENSITIVITY);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Update point cloud buffers
|
||||||
|
pointCloud_.mutexData.lock();
|
||||||
|
pointCloud_.update();
|
||||||
|
pointCloud_.mutexData.unlock();
|
||||||
|
camera_.update();
|
||||||
|
clearInputs();
|
||||||
|
}
|
||||||
|
|
||||||
|
void GLViewer::draw() {
|
||||||
|
const sl::Transform vpMatrix = camera_.getViewProjectionMatrix();
|
||||||
|
|
||||||
|
// Draw point cloud first
|
||||||
|
pointCloud_.draw(vpMatrix);
|
||||||
|
|
||||||
|
// Then draw frustum on top
|
||||||
|
glUseProgram(shader_.getProgramId());
|
||||||
|
glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
|
||||||
|
glUniformMatrix4fv(shMVPMatrixLoc_, 1, GL_FALSE, sl::Transform::transpose(vpMatrix * frustum.getModelMatrix()).m);
|
||||||
|
frustum.draw();
|
||||||
|
glUseProgram(0);
|
||||||
|
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
|
||||||
|
}
|
||||||
|
|
||||||
|
void GLViewer::clearInputs() {
|
||||||
|
mouseMotion_[0] = mouseMotion_[1] = 0;
|
||||||
|
mouseWheelPosition_ = 0;
|
||||||
|
for (unsigned int i = 0; i < 256; ++i)
|
||||||
|
if (keyStates_[i] != KEY_STATE::DOWN)
|
||||||
|
keyStates_[i] = KEY_STATE::FREE;
|
||||||
|
}
|
||||||
|
|
||||||
|
void GLViewer::drawCallback() {
|
||||||
|
currentInstance_->render();
|
||||||
|
}
|
||||||
|
|
||||||
|
void GLViewer::mouseButtonCallback(int button, int state, int x, int y) {
|
||||||
|
if (button < 5) {
|
||||||
|
if (button < 3) {
|
||||||
|
currentInstance_->mouseButton_[button] = state == GLUT_DOWN;
|
||||||
|
} else {
|
||||||
|
currentInstance_->mouseWheelPosition_ += button == MOUSE_BUTTON::WHEEL_UP ? 1 : -1;
|
||||||
|
}
|
||||||
|
currentInstance_->mouseCurrentPosition_[0] = x;
|
||||||
|
currentInstance_->mouseCurrentPosition_[1] = y;
|
||||||
|
currentInstance_->previousMouseMotion_[0] = x;
|
||||||
|
currentInstance_->previousMouseMotion_[1] = y;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void GLViewer::mouseMotionCallback(int x, int y) {
|
||||||
|
currentInstance_->mouseMotion_[0] = x - currentInstance_->previousMouseMotion_[0];
|
||||||
|
currentInstance_->mouseMotion_[1] = y - currentInstance_->previousMouseMotion_[1];
|
||||||
|
currentInstance_->previousMouseMotion_[0] = x;
|
||||||
|
currentInstance_->previousMouseMotion_[1] = y;
|
||||||
|
glutPostRedisplay();
|
||||||
|
}
|
||||||
|
|
||||||
|
void GLViewer::reshapeCallback(int width, int height) {
|
||||||
|
glViewport(0, 0, width, height);
|
||||||
|
float hfov = (180.0f / M_PI) * (2.0f * atan(width / (2.0f * 500)));
|
||||||
|
float vfov = (180.0f / M_PI) * (2.0f * atan(height / (2.0f * 500)));
|
||||||
|
currentInstance_->camera_.setProjection(hfov, vfov, currentInstance_->camera_.getZNear(), currentInstance_->camera_.getZFar());
|
||||||
|
}
|
||||||
|
|
||||||
|
void GLViewer::keyPressedCallback(unsigned char c, int x, int y) {
|
||||||
|
currentInstance_->keyStates_[c] = KEY_STATE::DOWN;
|
||||||
|
glutPostRedisplay();
|
||||||
|
}
|
||||||
|
|
||||||
|
void GLViewer::keyReleasedCallback(unsigned char c, int x, int y) {
|
||||||
|
currentInstance_->keyStates_[c] = KEY_STATE::UP;
|
||||||
|
}
|
||||||
|
|
||||||
|
void GLViewer::idle() {
|
||||||
|
glutPostRedisplay();
|
||||||
|
}
|
||||||
|
|
||||||
|
Simple3DObject::Simple3DObject() : vaoID_(0) {}
|
||||||
|
|
||||||
|
Simple3DObject::Simple3DObject(sl::Translation position, bool isStatic): isStatic_(isStatic) {
|
||||||
|
vaoID_ = 0;
|
||||||
|
drawingType_ = GL_TRIANGLES;
|
||||||
|
position_ = position;
|
||||||
|
rotation_.setIdentity();
|
||||||
|
}
|
||||||
|
|
||||||
|
Simple3DObject::~Simple3DObject() {
|
||||||
|
if (vaoID_ != 0) {
|
||||||
|
glDeleteBuffers(3, vboID_);
|
||||||
|
glDeleteVertexArrays(1, &vaoID_);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void Simple3DObject::addPoint(sl::float3 pt, sl::float3 clr) {
|
||||||
|
vertices_.push_back(pt.x);
|
||||||
|
vertices_.push_back(pt.y);
|
||||||
|
vertices_.push_back(pt.z);
|
||||||
|
colors_.push_back(clr.r);
|
||||||
|
colors_.push_back(clr.g);
|
||||||
|
colors_.push_back(clr.b);
|
||||||
|
indices_.push_back((int) indices_.size());
|
||||||
|
}
|
||||||
|
|
||||||
|
void Simple3DObject::addFace(sl::float3 p1, sl::float3 p2, sl::float3 p3, sl::float3 clr) {
|
||||||
|
vertices_.push_back(p1.x);
|
||||||
|
vertices_.push_back(p1.y);
|
||||||
|
vertices_.push_back(p1.z);
|
||||||
|
|
||||||
|
colors_.push_back(clr.r);
|
||||||
|
colors_.push_back(clr.g);
|
||||||
|
colors_.push_back(clr.b);
|
||||||
|
|
||||||
|
vertices_.push_back(p2.x);
|
||||||
|
vertices_.push_back(p2.y);
|
||||||
|
vertices_.push_back(p2.z);
|
||||||
|
|
||||||
|
colors_.push_back(clr.r);
|
||||||
|
colors_.push_back(clr.g);
|
||||||
|
colors_.push_back(clr.b);
|
||||||
|
|
||||||
|
vertices_.push_back(p3.x);
|
||||||
|
vertices_.push_back(p3.y);
|
||||||
|
vertices_.push_back(p3.z);
|
||||||
|
|
||||||
|
colors_.push_back(clr.r);
|
||||||
|
colors_.push_back(clr.g);
|
||||||
|
colors_.push_back(clr.b);
|
||||||
|
|
||||||
|
indices_.push_back((int) indices_.size());
|
||||||
|
indices_.push_back((int) indices_.size());
|
||||||
|
indices_.push_back((int) indices_.size());
|
||||||
|
}
|
||||||
|
|
||||||
|
void Simple3DObject::pushToGPU() {
|
||||||
|
if (!isStatic_ || vaoID_ == 0) {
|
||||||
|
if (vaoID_ == 0) {
|
||||||
|
glGenVertexArrays(1, &vaoID_);
|
||||||
|
glGenBuffers(3, vboID_);
|
||||||
|
}
|
||||||
|
glBindVertexArray(vaoID_);
|
||||||
|
glBindBuffer(GL_ARRAY_BUFFER, vboID_[0]);
|
||||||
|
glBufferData(GL_ARRAY_BUFFER, vertices_.size() * sizeof(float), &vertices_[0], isStatic_ ? GL_STATIC_DRAW : GL_DYNAMIC_DRAW);
|
||||||
|
glVertexAttribPointer(Shader::ATTRIB_VERTICES_POS, 3, GL_FLOAT, GL_FALSE, 0, 0);
|
||||||
|
glEnableVertexAttribArray(Shader::ATTRIB_VERTICES_POS);
|
||||||
|
|
||||||
|
glBindBuffer(GL_ARRAY_BUFFER, vboID_[1]);
|
||||||
|
glBufferData(GL_ARRAY_BUFFER, colors_.size() * sizeof(float), &colors_[0], isStatic_ ? GL_STATIC_DRAW : GL_DYNAMIC_DRAW);
|
||||||
|
glVertexAttribPointer(Shader::ATTRIB_COLOR_POS, 3, GL_FLOAT, GL_FALSE, 0, 0);
|
||||||
|
glEnableVertexAttribArray(Shader::ATTRIB_COLOR_POS);
|
||||||
|
|
||||||
|
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, vboID_[2]);
|
||||||
|
glBufferData(GL_ELEMENT_ARRAY_BUFFER, indices_.size() * sizeof(unsigned int), &indices_[0], isStatic_ ? GL_STATIC_DRAW : GL_DYNAMIC_DRAW);
|
||||||
|
|
||||||
|
glBindVertexArray(0);
|
||||||
|
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
|
||||||
|
glBindBuffer(GL_ARRAY_BUFFER, 0);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void Simple3DObject::clear() {
|
||||||
|
vertices_.clear();
|
||||||
|
colors_.clear();
|
||||||
|
indices_.clear();
|
||||||
|
}
|
||||||
|
|
||||||
|
void Simple3DObject::setDrawingType(GLenum type) {
|
||||||
|
drawingType_ = type;
|
||||||
|
}
|
||||||
|
|
||||||
|
void Simple3DObject::draw() {
|
||||||
|
glBindVertexArray(vaoID_);
|
||||||
|
glDrawElements(drawingType_, (GLsizei) indices_.size(), GL_UNSIGNED_INT, 0);
|
||||||
|
glBindVertexArray(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
void Simple3DObject::translate(const sl::Translation& t) {
|
||||||
|
position_ = position_ + t;
|
||||||
|
}
|
||||||
|
|
||||||
|
void Simple3DObject::setPosition(const sl::Translation& p) {
|
||||||
|
position_ = p;
|
||||||
|
}
|
||||||
|
|
||||||
|
void Simple3DObject::setRT(const sl::Transform& mRT) {
|
||||||
|
position_ = mRT.getTranslation();
|
||||||
|
rotation_ = mRT.getOrientation();
|
||||||
|
}
|
||||||
|
|
||||||
|
void Simple3DObject::rotate(const sl::Orientation& rot) {
|
||||||
|
rotation_ = rot * rotation_;
|
||||||
|
}
|
||||||
|
|
||||||
|
void Simple3DObject::rotate(const sl::Rotation& m) {
|
||||||
|
this->rotate(sl::Orientation(m));
|
||||||
|
}
|
||||||
|
|
||||||
|
void Simple3DObject::setRotation(const sl::Orientation& rot) {
|
||||||
|
rotation_ = rot;
|
||||||
|
}
|
||||||
|
|
||||||
|
void Simple3DObject::setRotation(const sl::Rotation& m) {
|
||||||
|
this->setRotation(sl::Orientation(m));
|
||||||
|
}
|
||||||
|
|
||||||
|
const sl::Translation& Simple3DObject::getPosition() const {
|
||||||
|
return position_;
|
||||||
|
}
|
||||||
|
|
||||||
|
sl::Transform Simple3DObject::getModelMatrix() const {
|
||||||
|
sl::Transform tmp;
|
||||||
|
tmp.setOrientation(rotation_);
|
||||||
|
tmp.setTranslation(position_);
|
||||||
|
return tmp;
|
||||||
|
}
|
||||||
|
|
||||||
|
Shader::Shader(const GLchar* vs, const GLchar* fs) {
|
||||||
|
set(vs, fs);
|
||||||
|
}
|
||||||
|
|
||||||
|
void Shader::set(const GLchar* vs, const GLchar* fs) {
|
||||||
|
if (!compile(verterxId_, GL_VERTEX_SHADER, vs)) {
|
||||||
|
print("ERROR: while compiling vertex shader");
|
||||||
|
}
|
||||||
|
if (!compile(fragmentId_, GL_FRAGMENT_SHADER, fs)) {
|
||||||
|
print("ERROR: while compiling fragment shader");
|
||||||
|
}
|
||||||
|
|
||||||
|
programId_ = glCreateProgram();
|
||||||
|
|
||||||
|
glAttachShader(programId_, verterxId_);
|
||||||
|
glAttachShader(programId_, fragmentId_);
|
||||||
|
|
||||||
|
glBindAttribLocation(programId_, ATTRIB_VERTICES_POS, "in_vertex");
|
||||||
|
glBindAttribLocation(programId_, ATTRIB_COLOR_POS, "in_texCoord");
|
||||||
|
|
||||||
|
glLinkProgram(programId_);
|
||||||
|
|
||||||
|
GLint errorlk(0);
|
||||||
|
glGetProgramiv(programId_, GL_LINK_STATUS, &errorlk);
|
||||||
|
if (errorlk != GL_TRUE) {
|
||||||
|
print("ERROR: while linking shader : ");
|
||||||
|
GLint errorSize(0);
|
||||||
|
glGetProgramiv(programId_, GL_INFO_LOG_LENGTH, &errorSize);
|
||||||
|
|
||||||
|
char *error = new char[errorSize + 1];
|
||||||
|
glGetShaderInfoLog(programId_, errorSize, &errorSize, error);
|
||||||
|
error[errorSize] = '\0';
|
||||||
|
std::cout << error << std::endl;
|
||||||
|
|
||||||
|
delete[] error;
|
||||||
|
glDeleteProgram(programId_);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
Shader::~Shader() {
|
||||||
|
if (verterxId_ != 0 && glIsShader(verterxId_))
|
||||||
|
glDeleteShader(verterxId_);
|
||||||
|
if (fragmentId_ != 0 && glIsShader(fragmentId_))
|
||||||
|
glDeleteShader(fragmentId_);
|
||||||
|
if (programId_ != 0 && glIsProgram(programId_))
|
||||||
|
glDeleteProgram(programId_);
|
||||||
|
}
|
||||||
|
|
||||||
|
GLuint Shader::getProgramId() {
|
||||||
|
return programId_;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool Shader::compile(GLuint &shaderId, GLenum type, const GLchar* src) {
|
||||||
|
shaderId = glCreateShader(type);
|
||||||
|
if (shaderId == 0) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
glShaderSource(shaderId, 1, (const char**) &src, 0);
|
||||||
|
glCompileShader(shaderId);
|
||||||
|
|
||||||
|
GLint errorCp(0);
|
||||||
|
glGetShaderiv(shaderId, GL_COMPILE_STATUS, &errorCp);
|
||||||
|
if (errorCp != GL_TRUE) {
|
||||||
|
print("ERROR: while compiling shader : ");
|
||||||
|
GLint errorSize(0);
|
||||||
|
glGetShaderiv(shaderId, GL_INFO_LOG_LENGTH, &errorSize);
|
||||||
|
|
||||||
|
char *error = new char[errorSize + 1];
|
||||||
|
glGetShaderInfoLog(shaderId, errorSize, &errorSize, error);
|
||||||
|
error[errorSize] = '\0';
|
||||||
|
std::cout << error << std::endl;
|
||||||
|
|
||||||
|
delete[] error;
|
||||||
|
glDeleteShader(shaderId);
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
const GLchar* POINTCLOUD_VERTEX_SHADER =
|
||||||
|
"#version 330 core\n"
|
||||||
|
"layout(location = 0) in vec4 in_VertexRGBA;\n"
|
||||||
|
"uniform mat4 u_mvpMatrix;\n"
|
||||||
|
"out vec4 b_color;\n"
|
||||||
|
"void main() {\n"
|
||||||
|
// Decompose the 4th channel of the XYZRGBA buffer to retrieve the color of the point (1float to 4uint)
|
||||||
|
" uint vertexColor = floatBitsToUint(in_VertexRGBA.w); \n"
|
||||||
|
" vec3 clr_int = vec3((vertexColor & uint(0x000000FF)), (vertexColor & uint(0x0000FF00)) >> 8, (vertexColor & uint(0x00FF0000)) >> 16);\n"
|
||||||
|
" b_color = vec4(clr_int.r / 255.0f, clr_int.g / 255.0f, clr_int.b / 255.0f, 1.f);"
|
||||||
|
// Scale from mm to m (divide by 1000) before applying MVP
|
||||||
|
" vec3 pos_meters = in_VertexRGBA.xyz * 0.001;\n"
|
||||||
|
" gl_Position = u_mvpMatrix * vec4(pos_meters, 1);\n"
|
||||||
|
"}";
|
||||||
|
|
||||||
|
const GLchar* POINTCLOUD_FRAGMENT_SHADER =
|
||||||
|
"#version 330 core\n"
|
||||||
|
"in vec4 b_color;\n"
|
||||||
|
"layout(location = 0) out vec4 out_Color;\n"
|
||||||
|
"void main() {\n"
|
||||||
|
" out_Color = b_color;\n"
|
||||||
|
"}";
|
||||||
|
|
||||||
|
PointCloud::PointCloud(): hasNewPCL_(false) {
|
||||||
|
}
|
||||||
|
|
||||||
|
PointCloud::~PointCloud() {
|
||||||
|
close();
|
||||||
|
}
|
||||||
|
|
||||||
|
void checkError(cudaError_t err) {
|
||||||
|
if(err != cudaSuccess)
|
||||||
|
std::cerr << "Error: (" << err << "): " << cudaGetErrorString(err) << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
void PointCloud::close() {
|
||||||
|
if (matGPU_.isInit()) {
|
||||||
|
matGPU_.free();
|
||||||
|
glDeleteBuffers(1, &bufferGLID_);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void PointCloud::initialize(sl::Resolution res, CUstream strm_) {
|
||||||
|
glGenBuffers(1, &bufferGLID_);
|
||||||
|
glBindBuffer(GL_ARRAY_BUFFER, bufferGLID_);
|
||||||
|
glBufferData(GL_ARRAY_BUFFER, res.area() * 4 * sizeof(float), 0, GL_DYNAMIC_DRAW);
|
||||||
|
glBindBuffer(GL_ARRAY_BUFFER, 0);
|
||||||
|
strm = strm_;
|
||||||
|
|
||||||
|
checkError(cudaGraphicsGLRegisterBuffer(&bufferCudaID_, bufferGLID_, cudaGraphicsRegisterFlagsWriteDiscard));
|
||||||
|
|
||||||
|
shader_.set(POINTCLOUD_VERTEX_SHADER, POINTCLOUD_FRAGMENT_SHADER);
|
||||||
|
shMVPMatrixLoc_ = glGetUniformLocation(shader_.getProgramId(), "u_mvpMatrix");
|
||||||
|
|
||||||
|
matGPU_.alloc(res, sl::MAT_TYPE::F32_C4, sl::MEM::GPU);
|
||||||
|
|
||||||
|
// Don't keep it mapped - we'll map/unmap in update()
|
||||||
|
numBytes_ = res.area() * 4 * sizeof(float);
|
||||||
|
}
|
||||||
|
|
||||||
|
void PointCloud::pushNewPC(sl::Mat &matXYZRGBA) {
|
||||||
|
if (matGPU_.isInit()) {
|
||||||
|
matGPU_.setFrom(matXYZRGBA, sl::COPY_TYPE::GPU_GPU, strm);
|
||||||
|
hasNewPCL_ = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void PointCloud::update() {
|
||||||
|
if (hasNewPCL_ && matGPU_.isInit()) {
|
||||||
|
// Map the resource for CUDA access
|
||||||
|
checkError(cudaGraphicsMapResources(1, &bufferCudaID_, strm));
|
||||||
|
checkError(cudaGraphicsResourceGetMappedPointer((void**) &xyzrgbaMappedBuf_, &numBytes_, bufferCudaID_));
|
||||||
|
|
||||||
|
// Copy data from ZED SDK to mapped OpenGL buffer
|
||||||
|
checkError(cudaMemcpyAsync(xyzrgbaMappedBuf_, matGPU_.getPtr<sl::float4>(sl::MEM::GPU), numBytes_, cudaMemcpyDeviceToDevice, strm));
|
||||||
|
|
||||||
|
// Synchronize to ensure copy completes
|
||||||
|
checkError(cudaStreamSynchronize(strm));
|
||||||
|
|
||||||
|
// Unmap so OpenGL can use it
|
||||||
|
checkError(cudaGraphicsUnmapResources(1, &bufferCudaID_, strm));
|
||||||
|
|
||||||
|
hasNewPCL_ = false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void PointCloud::draw(const sl::Transform& vp) {
|
||||||
|
if (matGPU_.isInit()) {
|
||||||
|
glDisable(GL_DEPTH_TEST);
|
||||||
|
glDisable(GL_CULL_FACE);
|
||||||
|
glPointSize(2.f);
|
||||||
|
glEnable(GL_PROGRAM_POINT_SIZE);
|
||||||
|
|
||||||
|
glUseProgram(shader_.getProgramId());
|
||||||
|
glUniformMatrix4fv(shMVPMatrixLoc_, 1, GL_FALSE, sl::Transform::transpose(vp).m);
|
||||||
|
|
||||||
|
glBindBuffer(GL_ARRAY_BUFFER, bufferGLID_);
|
||||||
|
glVertexAttribPointer(Shader::ATTRIB_VERTICES_POS, 4, GL_FLOAT, GL_FALSE, 0, 0);
|
||||||
|
glEnableVertexAttribArray(Shader::ATTRIB_VERTICES_POS);
|
||||||
|
|
||||||
|
int num_points = matGPU_.getResolution().area();
|
||||||
|
glDrawArrays(GL_POINTS, 0, num_points);
|
||||||
|
|
||||||
|
glDisableVertexAttribArray(Shader::ATTRIB_VERTICES_POS);
|
||||||
|
glBindBuffer(GL_ARRAY_BUFFER, 0);
|
||||||
|
glUseProgram(0);
|
||||||
|
|
||||||
|
glEnable(GL_DEPTH_TEST);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
const sl::Translation CameraGL::ORIGINAL_FORWARD = sl::Translation(0, 0, 1);
|
||||||
|
const sl::Translation CameraGL::ORIGINAL_UP = sl::Translation(0, 1, 0);
|
||||||
|
const sl::Translation CameraGL::ORIGINAL_RIGHT = sl::Translation(1, 0, 0);
|
||||||
|
|
||||||
|
CameraGL::CameraGL(sl::Translation position, sl::Translation direction, sl::Translation vertical) {
|
||||||
|
this->position_ = position;
|
||||||
|
setDirection(direction, vertical);
|
||||||
|
|
||||||
|
offset_ = sl::Translation(0, 0, 0);
|
||||||
|
view_.setIdentity();
|
||||||
|
updateView();
|
||||||
|
setProjection(90, 90, 0.01f, 50.f); // Near/far in meters (0.01m = 10mm, 50m = 50000mm)
|
||||||
|
updateVPMatrix();
|
||||||
|
}
|
||||||
|
|
||||||
|
CameraGL::~CameraGL() {}
|
||||||
|
|
||||||
|
void CameraGL::update() {
|
||||||
|
if (sl::Translation::dot(vertical_, up_) < 0)
|
||||||
|
vertical_ = vertical_ * -1.f;
|
||||||
|
updateView();
|
||||||
|
updateVPMatrix();
|
||||||
|
}
|
||||||
|
|
||||||
|
void CameraGL::setProjection(float horizontalFOV, float verticalFOV, float znear, float zfar) {
|
||||||
|
horizontalFieldOfView_ = horizontalFOV;
|
||||||
|
verticalFieldOfView_ = verticalFOV;
|
||||||
|
znear_ = znear;
|
||||||
|
zfar_ = zfar;
|
||||||
|
|
||||||
|
float fov_y = verticalFOV * M_PI / 180.f;
|
||||||
|
float fov_x = horizontalFOV * M_PI / 180.f;
|
||||||
|
|
||||||
|
projection_.setIdentity();
|
||||||
|
projection_(0, 0) = 1.0f / tanf(fov_x * 0.5f);
|
||||||
|
projection_(1, 1) = 1.0f / tanf(fov_y * 0.5f);
|
||||||
|
projection_(2, 2) = -(zfar + znear) / (zfar - znear);
|
||||||
|
projection_(3, 2) = -1;
|
||||||
|
projection_(2, 3) = -(2.f * zfar * znear) / (zfar - znear);
|
||||||
|
projection_(3, 3) = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
const sl::Transform& CameraGL::getViewProjectionMatrix() const {
|
||||||
|
return vpMatrix_;
|
||||||
|
}
|
||||||
|
|
||||||
|
float CameraGL::getHorizontalFOV() const {
|
||||||
|
return horizontalFieldOfView_;
|
||||||
|
}
|
||||||
|
|
||||||
|
float CameraGL::getVerticalFOV() const {
|
||||||
|
return verticalFieldOfView_;
|
||||||
|
}
|
||||||
|
|
||||||
|
void CameraGL::setOffsetFromPosition(const sl::Translation& o) {
|
||||||
|
offset_ = o;
|
||||||
|
}
|
||||||
|
|
||||||
|
const sl::Translation& CameraGL::getOffsetFromPosition() const {
|
||||||
|
return offset_;
|
||||||
|
}
|
||||||
|
|
||||||
|
void CameraGL::setDirection(const sl::Translation& direction, const sl::Translation& vertical) {
|
||||||
|
sl::Translation dirNormalized = direction;
|
||||||
|
dirNormalized.normalize();
|
||||||
|
this->rotation_ = sl::Orientation(ORIGINAL_FORWARD, dirNormalized * -1.f);
|
||||||
|
updateVectors();
|
||||||
|
this->vertical_ = vertical;
|
||||||
|
if (sl::Translation::dot(vertical_, up_) < 0)
|
||||||
|
rotate(sl::Rotation(M_PI, ORIGINAL_FORWARD));
|
||||||
|
}
|
||||||
|
|
||||||
|
void CameraGL::translate(const sl::Translation& t) {
|
||||||
|
position_ = position_ + t;
|
||||||
|
}
|
||||||
|
|
||||||
|
void CameraGL::setPosition(const sl::Translation& p) {
|
||||||
|
position_ = p;
|
||||||
|
}
|
||||||
|
|
||||||
|
void CameraGL::rotate(const sl::Orientation& rot) {
|
||||||
|
rotation_ = rot * rotation_;
|
||||||
|
updateVectors();
|
||||||
|
}
|
||||||
|
|
||||||
|
void CameraGL::rotate(const sl::Rotation& m) {
|
||||||
|
this->rotate(sl::Orientation(m));
|
||||||
|
}
|
||||||
|
|
||||||
|
void CameraGL::setRotation(const sl::Orientation& rot) {
|
||||||
|
rotation_ = rot;
|
||||||
|
updateVectors();
|
||||||
|
}
|
||||||
|
|
||||||
|
void CameraGL::setRotation(const sl::Rotation& m) {
|
||||||
|
this->setRotation(sl::Orientation(m));
|
||||||
|
}
|
||||||
|
|
||||||
|
const sl::Translation& CameraGL::getPosition() const {
|
||||||
|
return position_;
|
||||||
|
}
|
||||||
|
|
||||||
|
const sl::Translation& CameraGL::getForward() const {
|
||||||
|
return forward_;
|
||||||
|
}
|
||||||
|
|
||||||
|
const sl::Translation& CameraGL::getRight() const {
|
||||||
|
return right_;
|
||||||
|
}
|
||||||
|
|
||||||
|
const sl::Translation& CameraGL::getUp() const {
|
||||||
|
return up_;
|
||||||
|
}
|
||||||
|
|
||||||
|
const sl::Translation& CameraGL::getVertical() const {
|
||||||
|
return vertical_;
|
||||||
|
}
|
||||||
|
|
||||||
|
float CameraGL::getZNear() const {
|
||||||
|
return znear_;
|
||||||
|
}
|
||||||
|
|
||||||
|
float CameraGL::getZFar() const {
|
||||||
|
return zfar_;
|
||||||
|
}
|
||||||
|
|
||||||
|
void CameraGL::updateVectors() {
|
||||||
|
forward_ = ORIGINAL_FORWARD * rotation_;
|
||||||
|
up_ = ORIGINAL_UP * rotation_;
|
||||||
|
right_ = sl::Translation(ORIGINAL_RIGHT * -1.f) * rotation_;
|
||||||
|
}
|
||||||
|
|
||||||
|
void CameraGL::updateView() {
|
||||||
|
sl::Transform transformation(rotation_, (offset_ * rotation_) + position_);
|
||||||
|
view_ = sl::Transform::inverse(transformation);
|
||||||
|
}
|
||||||
|
|
||||||
|
void CameraGL::updateVPMatrix() {
|
||||||
|
vpMatrix_ = projection_ * view_;
|
||||||
|
}
|
||||||
345
new_playground/depth-sensing/src/main.cpp
Executable file
345
new_playground/depth-sensing/src/main.cpp
Executable file
@ -0,0 +1,345 @@
|
|||||||
|
///////////////////////////////////////////////////////////////////////////
|
||||||
|
//
|
||||||
|
// Copyright (c) 2025, STEREOLABS.
|
||||||
|
//
|
||||||
|
// All rights reserved.
|
||||||
|
//
|
||||||
|
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
|
||||||
|
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
|
||||||
|
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
|
||||||
|
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
|
||||||
|
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
|
||||||
|
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
|
||||||
|
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||||
|
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||||
|
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
//
|
||||||
|
///////////////////////////////////////////////////////////////////////////
|
||||||
|
|
||||||
|
/*********************************************************************
|
||||||
|
** This sample demonstrates how to capture a live 3D point cloud **
|
||||||
|
** with the ZED SDK and display the result in an OpenGL window. **
|
||||||
|
*********************************************************************/
|
||||||
|
|
||||||
|
// ZED includes
|
||||||
|
#include <sl/Camera.hpp>
|
||||||
|
|
||||||
|
// OpenCV includes
|
||||||
|
#include <opencv2/opencv.hpp>
|
||||||
|
|
||||||
|
// Sample includes
|
||||||
|
#include "GLViewer.hpp"
|
||||||
|
|
||||||
|
// Using std and sl namespaces
|
||||||
|
using namespace std;
|
||||||
|
using namespace sl;
|
||||||
|
|
||||||
|
std::string parseArgs(int argc, char **argv, sl::InitParameters& param);
|
||||||
|
|
||||||
|
// Debug flag for testing GLViewer with synthetic point cloud
|
||||||
|
bool DEBUG_MODE = false;
|
||||||
|
|
||||||
|
void generateTestPointCloud(sl::Mat& pc, sl::Resolution res) {
|
||||||
|
// Create a synthetic point cloud for testing
|
||||||
|
std::cout << "Generating test point cloud..." << std::endl;
|
||||||
|
|
||||||
|
// Allocate on CPU to fill data
|
||||||
|
sl::Mat pc_cpu(res, sl::MAT_TYPE::F32_C4, sl::MEM::CPU);
|
||||||
|
|
||||||
|
int width = res.width;
|
||||||
|
int height = res.height;
|
||||||
|
|
||||||
|
for (int y = 0; y < height; y++) {
|
||||||
|
for (int x = 0; x < width; x++) {
|
||||||
|
sl::float4 point;
|
||||||
|
|
||||||
|
// Create a grid in 3D space
|
||||||
|
float fx = (x / (float)width - 0.5f) * 5000; // X: -2500 to 2500
|
||||||
|
float fy = (y / (float)height - 0.5f) * 2000; // Y: -1000 to 1000
|
||||||
|
float fz = -3000 - (x % 50) * 20; // Z: varying depth for interest
|
||||||
|
|
||||||
|
point.x = fx;
|
||||||
|
point.y = fy;
|
||||||
|
point.z = fz;
|
||||||
|
|
||||||
|
// Rainbow colors based on position
|
||||||
|
float hue = (x / (float)width) * 360.0f;
|
||||||
|
float r, g, b;
|
||||||
|
if (hue < 60) {
|
||||||
|
r = 255; g = (hue / 60.0f) * 255; b = 0;
|
||||||
|
} else if (hue < 120) {
|
||||||
|
r = ((120 - hue) / 60.0f) * 255; g = 255; b = 0;
|
||||||
|
} else if (hue < 180) {
|
||||||
|
r = 0; g = 255; b = ((hue - 120) / 60.0f) * 255;
|
||||||
|
} else if (hue < 240) {
|
||||||
|
r = 0; g = ((240 - hue) / 60.0f) * 255; b = 255;
|
||||||
|
} else if (hue < 300) {
|
||||||
|
r = ((hue - 240) / 60.0f) * 255; g = 0; b = 255;
|
||||||
|
} else {
|
||||||
|
r = 255; g = 0; b = ((360 - hue) / 60.0f) * 255;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Pack RGBA into float
|
||||||
|
unsigned int rgba = ((unsigned int)r) |
|
||||||
|
(((unsigned int)g) << 8) |
|
||||||
|
(((unsigned int)b) << 16) |
|
||||||
|
(255 << 24);
|
||||||
|
point.w = *reinterpret_cast<float*>(&rgba);
|
||||||
|
|
||||||
|
pc_cpu.setValue(x, y, point);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Copy to GPU
|
||||||
|
pc_cpu.copyTo(pc, sl::COPY_TYPE::CPU_GPU);
|
||||||
|
std::cout << "Test point cloud generated: " << width << "x" << height << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
int main(int argc, char **argv) {
|
||||||
|
// Check for debug mode
|
||||||
|
for (int i = 1; i < argc; i++) {
|
||||||
|
if (string(argv[i]) == "--debug" || string(argv[i]) == "-d") {
|
||||||
|
DEBUG_MODE = true;
|
||||||
|
std::cout << "[DEBUG MODE] Using synthetic test point cloud" << std::endl;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
Camera zed;
|
||||||
|
// Set configuration parameters for the ZED
|
||||||
|
InitParameters init_parameters;
|
||||||
|
init_parameters.depth_mode = DEPTH_MODE::NEURAL_PLUS;
|
||||||
|
init_parameters.coordinate_system = COORDINATE_SYSTEM::RIGHT_HANDED_Y_UP; // OpenGL's coordinate system is right_handed
|
||||||
|
init_parameters.sdk_verbose = 1;
|
||||||
|
init_parameters.maximum_working_resolution = sl::Resolution(0, 0);
|
||||||
|
auto mask_path = parseArgs(argc, argv, init_parameters);
|
||||||
|
|
||||||
|
// Open the camera
|
||||||
|
auto returned_state = zed.open(init_parameters);
|
||||||
|
if (returned_state != ERROR_CODE::SUCCESS) {
|
||||||
|
print("Camera Open", returned_state, "Exit program.");
|
||||||
|
return EXIT_FAILURE;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Load optional region of interest to exclude irrelevant area of the image
|
||||||
|
if(!mask_path.empty()) {
|
||||||
|
sl::Mat mask_roi;
|
||||||
|
auto err = mask_roi.read(mask_path.c_str());
|
||||||
|
if(err == sl::ERROR_CODE::SUCCESS)
|
||||||
|
zed.setRegionOfInterest(mask_roi, {MODULE::ALL});
|
||||||
|
else
|
||||||
|
std::cout << "Error loading Region of Interest file: " << err << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
auto camera_config = zed.getCameraInformation().camera_configuration;
|
||||||
|
// print(camera_config);
|
||||||
|
std::cout << "Coordinate system: RIGHT_HANDED_Y_UP (Y is vertical axis)" << std::endl;
|
||||||
|
// Automatically set to the optimal resolution
|
||||||
|
sl::Resolution res(-1, -1);
|
||||||
|
|
||||||
|
Mat point_cloud;
|
||||||
|
zed.retrieveMeasure(point_cloud, MEASURE::XYZRGBA, MEM::GPU, res);
|
||||||
|
res = point_cloud.getResolution();
|
||||||
|
|
||||||
|
// Setup for RGB image retrieval
|
||||||
|
Mat image_left;
|
||||||
|
cv::Mat image_left_ocv(res.height, res.width, CV_8UC4);
|
||||||
|
string window_name = "ZED | RGB Camera View";
|
||||||
|
cv::namedWindow(window_name, cv::WINDOW_NORMAL);
|
||||||
|
|
||||||
|
// Setup for depth image retrieval
|
||||||
|
Mat depth_image_zed(res.width, res.height, MAT_TYPE::U8_C4);
|
||||||
|
cv::Mat depth_image_ocv(res.height, res.width, CV_8UC4);
|
||||||
|
string depth_window_name = "ZED | Depth View";
|
||||||
|
cv::namedWindow(depth_window_name, cv::WINDOW_NORMAL);
|
||||||
|
|
||||||
|
auto stream = zed.getCUDAStream();
|
||||||
|
|
||||||
|
// Point cloud viewer
|
||||||
|
GLViewer viewer;
|
||||||
|
// Initialize point cloud viewer
|
||||||
|
GLenum errgl = viewer.init(argc, argv, camera_config.calibration_parameters.left_cam, stream, res);
|
||||||
|
if (errgl != GLEW_OK) {
|
||||||
|
print("Error OpenGL: " + std::string((char*)glewGetErrorString(errgl)));
|
||||||
|
return EXIT_FAILURE;
|
||||||
|
}
|
||||||
|
|
||||||
|
RuntimeParameters runParameters;
|
||||||
|
// Setting the depth confidence parameters
|
||||||
|
runParameters.enable_depth = true;
|
||||||
|
runParameters.confidence_threshold = 100;
|
||||||
|
runParameters.texture_confidence_threshold = 100;
|
||||||
|
|
||||||
|
std::cout << "Depth confidence threshold: " << runParameters.confidence_threshold << std::endl;
|
||||||
|
std::cout << "Texture confidence threshold: " << runParameters.texture_confidence_threshold << std::endl;
|
||||||
|
std::cout << "Press on 's' for saving current .ply file" << std::endl;
|
||||||
|
// Main Loop
|
||||||
|
while (viewer.isAvailable()) {
|
||||||
|
// Check that a new image is successfully acquired
|
||||||
|
if (auto ret = zed.grab(runParameters); ret == ERROR_CODE::SUCCESS || DEBUG_MODE) {
|
||||||
|
if (!DEBUG_MODE) {
|
||||||
|
// Retrieve the current RGB camera view
|
||||||
|
zed.retrieveImage(image_left, VIEW::LEFT, MEM::CPU, res);
|
||||||
|
|
||||||
|
// Display RGB image in OpenCV window
|
||||||
|
cv::Mat image_left_ocv_display = cv::Mat(res.height, res.width, CV_8UC4, image_left.getPtr<sl::uchar1>(MEM::CPU));
|
||||||
|
cv::imshow(window_name, image_left_ocv_display);
|
||||||
|
|
||||||
|
// Retrieve and display the depth view
|
||||||
|
zed.retrieveImage(depth_image_zed, VIEW::DEPTH, MEM::CPU, res);
|
||||||
|
cv::Mat depth_image_ocv_display = cv::Mat(res.height, res.width, CV_8UC4, depth_image_zed.getPtr<sl::uchar1>(MEM::CPU));
|
||||||
|
cv::imshow(depth_window_name, depth_image_ocv_display);
|
||||||
|
|
||||||
|
cv::waitKey(1);
|
||||||
|
|
||||||
|
// retrieve the current 3D coloread point cloud in GPU
|
||||||
|
auto pc_ret = zed.retrieveMeasure(point_cloud, MEASURE::XYZRGBA, MEM::GPU, res);
|
||||||
|
|
||||||
|
if (pc_ret == ERROR_CODE::SUCCESS) {
|
||||||
|
// Point cloud retrieved successfully
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
// Debug mode: generate synthetic point cloud
|
||||||
|
static bool first_time = true;
|
||||||
|
if (first_time) {
|
||||||
|
generateTestPointCloud(point_cloud, res);
|
||||||
|
first_time = false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Process point cloud (both real and debug mode)
|
||||||
|
if (point_cloud.isInit()) {
|
||||||
|
viewer.updatePointCloud(point_cloud);
|
||||||
|
// Print debug info occasionally (every 30 frames)
|
||||||
|
static int frame_count = 0;
|
||||||
|
if (frame_count++ % 30 == 0) {
|
||||||
|
// Sample some points to check validity and range
|
||||||
|
Mat pc_cpu;
|
||||||
|
point_cloud.copyTo(pc_cpu, COPY_TYPE::GPU_CPU);
|
||||||
|
int valid_count = 0;
|
||||||
|
int sample_size = 100;
|
||||||
|
float min_x = FLT_MAX, max_x = -FLT_MAX, avg_x = 0;
|
||||||
|
float min_y = FLT_MAX, max_y = -FLT_MAX, avg_y = 0;
|
||||||
|
float min_z = FLT_MAX, max_z = -FLT_MAX, avg_z = 0;
|
||||||
|
for (int i = 0; i < sample_size; i++) {
|
||||||
|
int x = (i * pc_cpu.getWidth()) / sample_size;
|
||||||
|
int y = pc_cpu.getHeight() / 2;
|
||||||
|
sl::float4 point;
|
||||||
|
pc_cpu.getValue(x, y, &point);
|
||||||
|
if (std::isfinite(point.x) && std::isfinite(point.y) && std::isfinite(point.z)) {
|
||||||
|
valid_count++;
|
||||||
|
if (point.x < min_x) min_x = point.x;
|
||||||
|
if (point.x > max_x) max_x = point.x;
|
||||||
|
avg_x += point.x;
|
||||||
|
if (point.y < min_y) min_y = point.y;
|
||||||
|
if (point.y > max_y) max_y = point.y;
|
||||||
|
avg_y += point.y;
|
||||||
|
if (point.z < min_z) min_z = point.z;
|
||||||
|
if (point.z > max_z) max_z = point.z;
|
||||||
|
avg_z += point.z;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (valid_count > 0) { avg_x /= valid_count; avg_y /= valid_count; avg_z /= valid_count; }
|
||||||
|
std::cout << "\nPoint cloud: " << point_cloud.getWidth() << "x" << point_cloud.getHeight()
|
||||||
|
<< " pixels, Valid: " << valid_count << "/" << sample_size << std::endl
|
||||||
|
<< " X: [" << min_x << ", " << max_x << "] avg=" << avg_x
|
||||||
|
<< ", Y: [" << min_y << ", " << max_y << "] avg=" << avg_y
|
||||||
|
<< ", Z: [" << min_z << ", " << max_z << "] avg=" << avg_z << std::endl;
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
std::cout << "\nWarning: Point cloud is not initialized!" << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!DEBUG_MODE) {
|
||||||
|
std::cout << "FPS: " << zed.getCurrentFPS() << "\r" << std::flush;
|
||||||
|
} else {
|
||||||
|
std::cout << "FPS: [DEBUG MODE] \r" << std::flush;
|
||||||
|
}
|
||||||
|
|
||||||
|
if(viewer.shouldSaveData()){
|
||||||
|
sl::Mat point_cloud_to_save;
|
||||||
|
if (!DEBUG_MODE) {
|
||||||
|
zed.retrieveMeasure(point_cloud_to_save, MEASURE::XYZRGBA);
|
||||||
|
} else {
|
||||||
|
point_cloud.copyTo(point_cloud_to_save);
|
||||||
|
}
|
||||||
|
auto write_suceed = point_cloud_to_save.write("Pointcloud.ply");
|
||||||
|
if(write_suceed == sl::ERROR_CODE::SUCCESS)
|
||||||
|
std::cout << "Current .ply file saving succeed" << std::endl;
|
||||||
|
else
|
||||||
|
std::cout << "Current .ply file saving failed" << std::endl;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
// free allocated memory before closing the ZED
|
||||||
|
point_cloud.free();
|
||||||
|
|
||||||
|
// close the ZED
|
||||||
|
zed.close();
|
||||||
|
|
||||||
|
return EXIT_SUCCESS;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline int findImageExtension(int argc, char **argv) {
|
||||||
|
int arg_idx=-1;
|
||||||
|
int arg_idx_search = 0;
|
||||||
|
if (argc > 2) arg_idx_search=2;
|
||||||
|
else if(argc > 1) arg_idx_search=1;
|
||||||
|
|
||||||
|
if(arg_idx_search > 0 && (string(argv[arg_idx_search]).find(".png") != string::npos ||
|
||||||
|
string(argv[arg_idx_search]).find(".jpg") != string::npos))
|
||||||
|
arg_idx = arg_idx_search;
|
||||||
|
return arg_idx;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
std::string parseArgs(int argc, char **argv, sl::InitParameters& param) {
|
||||||
|
int mask_arg = findImageExtension(argc, argv);
|
||||||
|
std::string mask_path;
|
||||||
|
|
||||||
|
if (argc > 1 && string(argv[1]).find(".svo") != string::npos) {
|
||||||
|
// SVO input mode
|
||||||
|
param.input.setFromSVOFile(argv[1]);
|
||||||
|
cout << "[Sample] Using SVO File input: " << argv[1] << endl;
|
||||||
|
} else if (argc > 1 && string(argv[1]).find(".svo") == string::npos) {
|
||||||
|
string arg = string(argv[1]);
|
||||||
|
unsigned int a, b, c, d, port;
|
||||||
|
if (sscanf(arg.c_str(), "%u.%u.%u.%u:%d", &a, &b, &c, &d, &port) == 5) {
|
||||||
|
// Stream input mode - IP + port
|
||||||
|
string ip_adress = to_string(a) + "." + to_string(b) + "." + to_string(c) + "." + to_string(d);
|
||||||
|
param.input.setFromStream(sl::String(ip_adress.c_str()), port);
|
||||||
|
cout << "[Sample] Using Stream input, IP : " << ip_adress << ", port : " << port << endl;
|
||||||
|
} else if (sscanf(arg.c_str(), "%u.%u.%u.%u", &a, &b, &c, &d) == 4) {
|
||||||
|
// Stream input mode - IP only
|
||||||
|
param.input.setFromStream(sl::String(argv[1]));
|
||||||
|
cout << "[Sample] Using Stream input, IP : " << argv[1] << endl;
|
||||||
|
}else if (arg.find("HD2K") != string::npos) {
|
||||||
|
param.camera_resolution = RESOLUTION::HD2K;
|
||||||
|
cout << "[Sample] Using Camera in resolution HD2K" << endl;
|
||||||
|
}else if (arg.find("HD1200") != string::npos) {
|
||||||
|
param.camera_resolution = RESOLUTION::HD1200;
|
||||||
|
cout << "[Sample] Using Camera in resolution HD1200" << endl;
|
||||||
|
} else if (arg.find("HD1080") != string::npos) {
|
||||||
|
param.camera_resolution = RESOLUTION::HD1080;
|
||||||
|
cout << "[Sample] Using Camera in resolution HD1080" << endl;
|
||||||
|
} else if (arg.find("HD720") != string::npos) {
|
||||||
|
param.camera_resolution = RESOLUTION::HD720;
|
||||||
|
cout << "[Sample] Using Camera in resolution HD720" << endl;
|
||||||
|
}else if (arg.find("SVGA") != string::npos) {
|
||||||
|
param.camera_resolution = RESOLUTION::SVGA;
|
||||||
|
cout << "[Sample] Using Camera in resolution SVGA" << endl;
|
||||||
|
}else if (arg.find("VGA") != string::npos) {
|
||||||
|
param.camera_resolution = RESOLUTION::VGA;
|
||||||
|
cout << "[Sample] Using Camera in resolution VGA" << endl;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (mask_arg > 0) {
|
||||||
|
mask_path = string(argv[mask_arg]);
|
||||||
|
cout << "[Sample] Using Region of Interest from file : " << mask_path << endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
return mask_path;
|
||||||
|
}
|
||||||
228
new_playground/depth-sensing/test_opengl.cpp
Executable file
228
new_playground/depth-sensing/test_opengl.cpp
Executable file
@ -0,0 +1,228 @@
|
|||||||
|
#include <GL/glew.h>
|
||||||
|
#include <GL/freeglut.h>
|
||||||
|
#include <iostream>
|
||||||
|
#include <cmath>
|
||||||
|
|
||||||
|
const char* VERTEX_SHADER =
|
||||||
|
"#version 330 core\n"
|
||||||
|
"layout(location = 0) in vec3 position;\n"
|
||||||
|
"layout(location = 1) in vec3 color;\n"
|
||||||
|
"uniform mat4 mvp;\n"
|
||||||
|
"out vec3 fragColor;\n"
|
||||||
|
"void main() {\n"
|
||||||
|
" fragColor = color;\n"
|
||||||
|
" gl_Position = mvp * vec4(position, 1.0);\n"
|
||||||
|
"}\n";
|
||||||
|
|
||||||
|
const char* FRAGMENT_SHADER =
|
||||||
|
"#version 330 core\n"
|
||||||
|
"in vec3 fragColor;\n"
|
||||||
|
"out vec4 outColor;\n"
|
||||||
|
"void main() {\n"
|
||||||
|
" outColor = vec4(fragColor, 1.0);\n"
|
||||||
|
"}\n";
|
||||||
|
|
||||||
|
GLuint shaderProgram;
|
||||||
|
GLuint vao, vbo_pos, vbo_color;
|
||||||
|
float angle = 0.0f;
|
||||||
|
|
||||||
|
GLuint compileShader(const char* source, GLenum type) {
|
||||||
|
GLuint shader = glCreateShader(type);
|
||||||
|
glShaderSource(shader, 1, &source, NULL);
|
||||||
|
glCompileShader(shader);
|
||||||
|
|
||||||
|
GLint success;
|
||||||
|
glGetShaderiv(shader, GL_COMPILE_STATUS, &success);
|
||||||
|
if (!success) {
|
||||||
|
char log[512];
|
||||||
|
glGetShaderInfoLog(shader, 512, NULL, log);
|
||||||
|
std::cerr << "Shader compilation error: " << log << std::endl;
|
||||||
|
}
|
||||||
|
return shader;
|
||||||
|
}
|
||||||
|
|
||||||
|
void createShaderProgram() {
|
||||||
|
GLuint vertShader = compileShader(VERTEX_SHADER, GL_VERTEX_SHADER);
|
||||||
|
GLuint fragShader = compileShader(FRAGMENT_SHADER, GL_FRAGMENT_SHADER);
|
||||||
|
|
||||||
|
shaderProgram = glCreateProgram();
|
||||||
|
glAttachShader(shaderProgram, vertShader);
|
||||||
|
glAttachShader(shaderProgram, fragShader);
|
||||||
|
glLinkProgram(shaderProgram);
|
||||||
|
|
||||||
|
GLint success;
|
||||||
|
glGetProgramiv(shaderProgram, GL_LINK_STATUS, &success);
|
||||||
|
if (!success) {
|
||||||
|
char log[512];
|
||||||
|
glGetProgramInfoLog(shaderProgram, 512, NULL, log);
|
||||||
|
std::cerr << "Shader linking error: " << log << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
glDeleteShader(vertShader);
|
||||||
|
glDeleteShader(fragShader);
|
||||||
|
}
|
||||||
|
|
||||||
|
void createGeometry() {
|
||||||
|
// Simple triangle vertices
|
||||||
|
float vertices[] = {
|
||||||
|
// Triangle 1 (large, in front)
|
||||||
|
0.0f, 0.5f, 0.0f,
|
||||||
|
-0.5f, -0.5f, 0.0f,
|
||||||
|
0.5f, -0.5f, 0.0f,
|
||||||
|
|
||||||
|
// Triangle 2 (smaller, behind)
|
||||||
|
0.0f, 0.3f, -2.0f,
|
||||||
|
-0.3f, -0.3f, -2.0f,
|
||||||
|
0.3f, -0.3f, -2.0f
|
||||||
|
};
|
||||||
|
|
||||||
|
float colors[] = {
|
||||||
|
// Triangle 1 colors (RGB at vertices)
|
||||||
|
1.0f, 0.0f, 0.0f, // Red
|
||||||
|
0.0f, 1.0f, 0.0f, // Green
|
||||||
|
0.0f, 0.0f, 1.0f, // Blue
|
||||||
|
|
||||||
|
// Triangle 2 colors (Yellow)
|
||||||
|
1.0f, 1.0f, 0.0f,
|
||||||
|
1.0f, 1.0f, 0.0f,
|
||||||
|
1.0f, 1.0f, 0.0f
|
||||||
|
};
|
||||||
|
|
||||||
|
glGenVertexArrays(1, &vao);
|
||||||
|
glBindVertexArray(vao);
|
||||||
|
|
||||||
|
glGenBuffers(1, &vbo_pos);
|
||||||
|
glBindBuffer(GL_ARRAY_BUFFER, vbo_pos);
|
||||||
|
glBufferData(GL_ARRAY_BUFFER, sizeof(vertices), vertices, GL_STATIC_DRAW);
|
||||||
|
glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, 0);
|
||||||
|
glEnableVertexAttribArray(0);
|
||||||
|
|
||||||
|
glGenBuffers(1, &vbo_color);
|
||||||
|
glBindBuffer(GL_ARRAY_BUFFER, vbo_color);
|
||||||
|
glBufferData(GL_ARRAY_BUFFER, sizeof(colors), colors, GL_STATIC_DRAW);
|
||||||
|
glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, 0, 0);
|
||||||
|
glEnableVertexAttribArray(1);
|
||||||
|
|
||||||
|
glBindVertexArray(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
void display() {
|
||||||
|
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
|
||||||
|
|
||||||
|
// Test 1: Draw with identity matrix (no depth)
|
||||||
|
static bool testDepth = true;
|
||||||
|
|
||||||
|
if (!testDepth) {
|
||||||
|
// Identity - everything should be visible
|
||||||
|
float mvp[16] = {
|
||||||
|
1.0f, 0.0f, 0.0f, 0.0f,
|
||||||
|
0.0f, 1.0f, 0.0f, 0.0f,
|
||||||
|
0.0f, 0.0f, 1.0f, 0.0f,
|
||||||
|
0.0f, 0.0f, 0.0f, 1.0f
|
||||||
|
};
|
||||||
|
|
||||||
|
glUseProgram(shaderProgram);
|
||||||
|
GLint mvpLoc = glGetUniformLocation(shaderProgram, "mvp");
|
||||||
|
glUniformMatrix4fv(mvpLoc, 1, GL_FALSE, mvp);
|
||||||
|
|
||||||
|
glBindVertexArray(vao);
|
||||||
|
glDrawArrays(GL_TRIANGLES, 0, 6);
|
||||||
|
glBindVertexArray(0);
|
||||||
|
} else {
|
||||||
|
// Test depth - draw with simple Z translation only
|
||||||
|
glUseProgram(shaderProgram);
|
||||||
|
GLint mvpLoc = glGetUniformLocation(shaderProgram, "mvp");
|
||||||
|
|
||||||
|
// Draw first triangle (at Z=0)
|
||||||
|
float mvp1[16] = {
|
||||||
|
1.0f, 0.0f, 0.0f, 0.0f,
|
||||||
|
0.0f, 1.0f, 0.0f, 0.0f,
|
||||||
|
0.0f, 0.0f, 1.0f, 0.0f,
|
||||||
|
0.0f, 0.0f, 0.0f, 1.0f
|
||||||
|
};
|
||||||
|
glUniformMatrix4fv(mvpLoc, 1, GL_FALSE, mvp1);
|
||||||
|
glBindVertexArray(vao);
|
||||||
|
glDrawArrays(GL_TRIANGLES, 0, 3);
|
||||||
|
|
||||||
|
// Draw second triangle shifted right and with Z offset
|
||||||
|
float mvp2[16] = {
|
||||||
|
1.0f, 0.0f, 0.0f, 0.0f,
|
||||||
|
0.0f, 1.0f, 0.0f, 0.0f,
|
||||||
|
0.0f, 0.0f, 1.0f, 0.0f,
|
||||||
|
0.4f, 0.0f, -0.5f, 1.0f // Shift right and back in Z
|
||||||
|
};
|
||||||
|
glUniformMatrix4fv(mvpLoc, 1, GL_FALSE, mvp2);
|
||||||
|
glDrawArrays(GL_TRIANGLES, 3, 3);
|
||||||
|
|
||||||
|
glBindVertexArray(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
glutSwapBuffers();
|
||||||
|
|
||||||
|
static int frames = 0;
|
||||||
|
if (frames++ == 0) {
|
||||||
|
std::cout << "\n=== Drawing Test ===" << std::endl;
|
||||||
|
std::cout << "You should see TWO triangles:" << std::endl;
|
||||||
|
std::cout << "1. RGB triangle on the left (Z=0)" << std::endl;
|
||||||
|
std::cout << "2. Yellow triangle on the right (Z=-0.5)" << std::endl;
|
||||||
|
std::cout << "\nIf you only see ONE triangle, depth is broken" << std::endl;
|
||||||
|
std::cout << "Press 'd' to toggle depth test" << std::endl;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void keyboard(unsigned char key, int x, int y) {
|
||||||
|
if (key == 'd' || key == 'D') {
|
||||||
|
static bool depthEnabled = true;
|
||||||
|
depthEnabled = !depthEnabled;
|
||||||
|
if (depthEnabled) {
|
||||||
|
glEnable(GL_DEPTH_TEST);
|
||||||
|
std::cout << "Depth test ENABLED" << std::endl;
|
||||||
|
} else {
|
||||||
|
glDisable(GL_DEPTH_TEST);
|
||||||
|
std::cout << "Depth test DISABLED" << std::endl;
|
||||||
|
}
|
||||||
|
glutPostRedisplay();
|
||||||
|
} else if (key == 27) { // ESC
|
||||||
|
exit(0);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void init() {
|
||||||
|
std::cout << "GL Vendor: " << glGetString(GL_VENDOR) << std::endl;
|
||||||
|
std::cout << "GL Renderer: " << glGetString(GL_RENDERER) << std::endl;
|
||||||
|
std::cout << "GL Version: " << glGetString(GL_VERSION) << std::endl;
|
||||||
|
std::cout << "GLSL Version: " << glGetString(GL_SHADING_LANGUAGE_VERSION) << std::endl;
|
||||||
|
|
||||||
|
glClearColor(0.2f, 0.2f, 0.3f, 1.0f);
|
||||||
|
glEnable(GL_DEPTH_TEST);
|
||||||
|
|
||||||
|
createShaderProgram();
|
||||||
|
createGeometry();
|
||||||
|
}
|
||||||
|
|
||||||
|
int main(int argc, char** argv) {
|
||||||
|
glutInit(&argc, argv);
|
||||||
|
glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH);
|
||||||
|
glutInitWindowSize(800, 600);
|
||||||
|
glutCreateWindow("Minimal OpenGL Test");
|
||||||
|
|
||||||
|
GLenum err = glewInit();
|
||||||
|
if (err != GLEW_OK) {
|
||||||
|
std::cerr << "GLEW Init Error: " << glewGetErrorString(err) << std::endl;
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
init();
|
||||||
|
|
||||||
|
glutDisplayFunc(display);
|
||||||
|
glutKeyboardFunc(keyboard);
|
||||||
|
|
||||||
|
std::cout << "\n=== Minimal OpenGL Test ===" << std::endl;
|
||||||
|
std::cout << "Controls:" << std::endl;
|
||||||
|
std::cout << " d - Toggle depth test" << std::endl;
|
||||||
|
std::cout << " ESC - Quit" << std::endl;
|
||||||
|
|
||||||
|
glutMainLoop();
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
176
new_playground/depth-sensing/test_points.cpp
Executable file
176
new_playground/depth-sensing/test_points.cpp
Executable file
@ -0,0 +1,176 @@
|
|||||||
|
#include <GL/glew.h>
|
||||||
|
#include <GL/freeglut.h>
|
||||||
|
#include <iostream>
|
||||||
|
#include <vector>
|
||||||
|
#include <cmath>
|
||||||
|
|
||||||
|
const char* VERTEX_SHADER =
|
||||||
|
"#version 330 core\n"
|
||||||
|
"layout(location = 0) in vec3 position;\n"
|
||||||
|
"layout(location = 1) in vec3 color;\n"
|
||||||
|
"out vec3 fragColor;\n"
|
||||||
|
"void main() {\n"
|
||||||
|
" fragColor = color;\n"
|
||||||
|
" gl_Position = vec4(position, 1.0);\n"
|
||||||
|
" gl_PointSize = 5.0;\n"
|
||||||
|
"}\n";
|
||||||
|
|
||||||
|
const char* FRAGMENT_SHADER =
|
||||||
|
"#version 330 core\n"
|
||||||
|
"in vec3 fragColor;\n"
|
||||||
|
"out vec4 outColor;\n"
|
||||||
|
"void main() {\n"
|
||||||
|
" outColor = vec4(fragColor, 1.0);\n"
|
||||||
|
"}\n";
|
||||||
|
|
||||||
|
GLuint shaderProgram;
|
||||||
|
GLuint vao, vbo_pos, vbo_color;
|
||||||
|
int numPoints = 10000;
|
||||||
|
|
||||||
|
GLuint compileShader(const char* source, GLenum type) {
|
||||||
|
GLuint shader = glCreateShader(type);
|
||||||
|
glShaderSource(shader, 1, &source, NULL);
|
||||||
|
glCompileShader(shader);
|
||||||
|
|
||||||
|
GLint success;
|
||||||
|
glGetShaderiv(shader, GL_COMPILE_STATUS, &success);
|
||||||
|
if (!success) {
|
||||||
|
char log[512];
|
||||||
|
glGetShaderInfoLog(shader, 512, NULL, log);
|
||||||
|
std::cerr << "Shader compilation error: " << log << std::endl;
|
||||||
|
}
|
||||||
|
return shader;
|
||||||
|
}
|
||||||
|
|
||||||
|
void createShaderProgram() {
|
||||||
|
GLuint vertShader = compileShader(VERTEX_SHADER, GL_VERTEX_SHADER);
|
||||||
|
GLuint fragShader = compileShader(FRAGMENT_SHADER, GL_FRAGMENT_SHADER);
|
||||||
|
|
||||||
|
shaderProgram = glCreateProgram();
|
||||||
|
glAttachShader(shaderProgram, vertShader);
|
||||||
|
glAttachShader(shaderProgram, fragShader);
|
||||||
|
glLinkProgram(shaderProgram);
|
||||||
|
|
||||||
|
GLint success;
|
||||||
|
glGetProgramiv(shaderProgram, GL_LINK_STATUS, &success);
|
||||||
|
if (!success) {
|
||||||
|
char log[512];
|
||||||
|
glGetProgramInfoLog(shaderProgram, 512, NULL, log);
|
||||||
|
std::cerr << "Shader linking error: " << log << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
glDeleteShader(vertShader);
|
||||||
|
glDeleteShader(fragShader);
|
||||||
|
}
|
||||||
|
|
||||||
|
void createPointCloud() {
|
||||||
|
std::vector<float> vertices;
|
||||||
|
std::vector<float> colors;
|
||||||
|
|
||||||
|
// Create a grid of points
|
||||||
|
for (int i = 0; i < 100; i++) {
|
||||||
|
for (int j = 0; j < 100; j++) {
|
||||||
|
// X: -0.8 to 0.8, Y: -0.8 to 0.8, Z: 0
|
||||||
|
float x = -0.8f + (i / 100.0f) * 1.6f;
|
||||||
|
float y = -0.8f + (j / 100.0f) * 1.6f;
|
||||||
|
float z = 0.0f;
|
||||||
|
|
||||||
|
vertices.push_back(x);
|
||||||
|
vertices.push_back(y);
|
||||||
|
vertices.push_back(z);
|
||||||
|
|
||||||
|
// Color based on position (rainbow)
|
||||||
|
float r = (float)i / 100.0f;
|
||||||
|
float g = (float)j / 100.0f;
|
||||||
|
float b = 1.0f - (r + g) / 2.0f;
|
||||||
|
|
||||||
|
colors.push_back(r);
|
||||||
|
colors.push_back(g);
|
||||||
|
colors.push_back(b);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
glGenVertexArrays(1, &vao);
|
||||||
|
glBindVertexArray(vao);
|
||||||
|
|
||||||
|
glGenBuffers(1, &vbo_pos);
|
||||||
|
glBindBuffer(GL_ARRAY_BUFFER, vbo_pos);
|
||||||
|
glBufferData(GL_ARRAY_BUFFER, vertices.size() * sizeof(float), vertices.data(), GL_STATIC_DRAW);
|
||||||
|
glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, 0);
|
||||||
|
glEnableVertexAttribArray(0);
|
||||||
|
|
||||||
|
glGenBuffers(1, &vbo_color);
|
||||||
|
glBindBuffer(GL_ARRAY_BUFFER, vbo_color);
|
||||||
|
glBufferData(GL_ARRAY_BUFFER, colors.size() * sizeof(float), colors.data(), GL_STATIC_DRAW);
|
||||||
|
glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, 0, 0);
|
||||||
|
glEnableVertexAttribArray(1);
|
||||||
|
|
||||||
|
glBindVertexArray(0);
|
||||||
|
|
||||||
|
std::cout << "Created " << numPoints << " points" << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
void display() {
|
||||||
|
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
|
||||||
|
|
||||||
|
glUseProgram(shaderProgram);
|
||||||
|
glBindVertexArray(vao);
|
||||||
|
|
||||||
|
glEnable(GL_PROGRAM_POINT_SIZE);
|
||||||
|
glPointSize(5.0f);
|
||||||
|
|
||||||
|
glDrawArrays(GL_POINTS, 0, numPoints);
|
||||||
|
|
||||||
|
glBindVertexArray(0);
|
||||||
|
glUseProgram(0);
|
||||||
|
|
||||||
|
glutSwapBuffers();
|
||||||
|
|
||||||
|
static int frameCount = 0;
|
||||||
|
if (frameCount++ % 60 == 0) {
|
||||||
|
GLenum err = glGetError();
|
||||||
|
if (err != GL_NO_ERROR) {
|
||||||
|
std::cout << "OpenGL Error: " << err << std::endl;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void init() {
|
||||||
|
std::cout << "\n=== OpenGL Point Cloud Test ===" << std::endl;
|
||||||
|
std::cout << "GL Vendor: " << glGetString(GL_VENDOR) << std::endl;
|
||||||
|
std::cout << "GL Renderer: " << glGetString(GL_RENDERER) << std::endl;
|
||||||
|
std::cout << "GL Version: " << glGetString(GL_VERSION) << std::endl;
|
||||||
|
std::cout << "GLSL Version: " << glGetString(GL_SHADING_LANGUAGE_VERSION) << std::endl;
|
||||||
|
|
||||||
|
glClearColor(0.1f, 0.1f, 0.15f, 1.0f);
|
||||||
|
glEnable(GL_DEPTH_TEST);
|
||||||
|
|
||||||
|
createShaderProgram();
|
||||||
|
createPointCloud();
|
||||||
|
|
||||||
|
std::cout << "\nYou should see a 100x100 grid of colored points" << std::endl;
|
||||||
|
std::cout << "Points should have rainbow colors across the grid" << std::endl;
|
||||||
|
std::cout << "If you see nothing, OpenGL point rendering may not work via X11" << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
int main(int argc, char** argv) {
|
||||||
|
glutInit(&argc, argv);
|
||||||
|
glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH);
|
||||||
|
glutInitWindowSize(800, 600);
|
||||||
|
glutCreateWindow("OpenGL Point Test");
|
||||||
|
|
||||||
|
GLenum err = glewInit();
|
||||||
|
if (err != GLEW_OK) {
|
||||||
|
std::cerr << "GLEW Init Error: " << glewGetErrorString(err) << std::endl;
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
init();
|
||||||
|
|
||||||
|
glutDisplayFunc(display);
|
||||||
|
glutIdleFunc(display);
|
||||||
|
|
||||||
|
glutMainLoop();
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
Reference in New Issue
Block a user