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:
+778
@@ -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_;
|
||||
}
|
||||
Executable
+345
@@ -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;
|
||||
}
|
||||
Reference in New Issue
Block a user