crosstyan
e7c5176229
- 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.
779 lines
23 KiB
C++
Executable File
779 lines
23 KiB
C++
Executable File
#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_;
|
|
}
|