337 lines
8.1 KiB
C++
Executable File
337 lines
8.1 KiB
C++
Executable File
#ifndef __VIEWER_INCLUDE__
|
|
#define __VIEWER_INCLUDE__
|
|
|
|
#include <random>
|
|
|
|
#include <sl/Camera.hpp>
|
|
#include <sl/Fusion.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.75f
|
|
#define MOUSE_DZ_SENSITIVITY 1.25f
|
|
#define MOUSE_T_SENSITIVITY 0.05f
|
|
#define KEY_T_SENSITIVITY 0.1f
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////////////////////////
|
|
|
|
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;
|
|
static const GLint ATTRIB_NORMAL = 2;
|
|
private:
|
|
bool compile(GLuint &shaderId, GLenum type, const GLchar* src);
|
|
GLuint verterxId_;
|
|
GLuint fragmentId_;
|
|
GLuint programId_;
|
|
};
|
|
|
|
struct ShaderData {
|
|
Shader it;
|
|
GLuint MVP_Mat;
|
|
};
|
|
|
|
class Simple3DObject {
|
|
public:
|
|
|
|
Simple3DObject();
|
|
|
|
~Simple3DObject();
|
|
|
|
void addPoint(sl::float3 pt, sl::float3 clr);
|
|
void addLine(sl::float3 pt1, sl::float3 pt2, sl::float3 clr);
|
|
void addFace(sl::float3 p1, sl::float3 p2, sl::float3 p3, sl::float3 clr);
|
|
void pushToGPU();
|
|
void clear();
|
|
|
|
void setStatic(bool _static) {
|
|
isStatic_ = _static;
|
|
}
|
|
|
|
void setDrawingType(GLenum type);
|
|
|
|
void draw();
|
|
|
|
private:
|
|
std::vector<sl::float3> vertices_;
|
|
std::vector<sl::float3> colors_;
|
|
std::vector<unsigned int> indices_;
|
|
|
|
bool isStatic_;
|
|
bool need_update;
|
|
GLenum drawingType_;
|
|
GLuint vaoID_;
|
|
GLuint vboID_[3];
|
|
};
|
|
|
|
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_;
|
|
bool usePerspective_;
|
|
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 PointCloud {
|
|
public:
|
|
PointCloud();
|
|
~PointCloud();
|
|
|
|
// Initialize Opengl and Cuda buffers
|
|
// Warning: must be called in the Opengl thread
|
|
void initialize(sl::Mat&, sl::float3 clr);
|
|
// Push a new point cloud
|
|
// Warning: can be called from any thread but the mutex "mutexData" must be locked
|
|
void pushNewPC();
|
|
// Draw the point cloud
|
|
// Warning: must be called in the Opengl thread
|
|
void draw(const sl::Transform& vp, bool draw_clr);
|
|
// Close (disable update)
|
|
void close();
|
|
|
|
private:
|
|
sl::Mat refMat;
|
|
sl::float3 clr;
|
|
|
|
Shader shader_;
|
|
GLuint shMVPMatrixLoc_;
|
|
GLuint shDrawColor;
|
|
GLuint shColor;
|
|
|
|
size_t numBytes_;
|
|
float* xyzrgbaMappedBuf_;
|
|
GLuint bufferGLID_;
|
|
cudaGraphicsResource* bufferCudaID_;
|
|
};
|
|
|
|
class CameraViewer {
|
|
public:
|
|
CameraViewer();
|
|
~CameraViewer();
|
|
|
|
// Initialize Opengl and Cuda buffers
|
|
bool initialize(sl::Mat& image, sl::float3 clr);
|
|
// Push a new Image + Z buffer and transform into a point cloud
|
|
void pushNewImage();
|
|
// Draw the Image
|
|
void draw(sl::Transform vpMatrix);
|
|
// Close (disable update)
|
|
void close();
|
|
|
|
Simple3DObject frustum;
|
|
private:
|
|
sl::Mat ref;
|
|
cudaArray_t ArrIm;
|
|
cudaGraphicsResource* cuda_gl_ressource;//cuda GL resource
|
|
Shader shader;
|
|
GLuint shMVPMatrixLocTex_;
|
|
|
|
GLuint texture;
|
|
GLuint vaoID_;
|
|
GLuint vboID_[3];
|
|
|
|
std::vector<sl::uint3> faces;
|
|
std::vector<sl::float3> vert;
|
|
std::vector<sl::float2> uv;
|
|
};
|
|
|
|
struct ObjectClassName {
|
|
sl::float3 position;
|
|
std::string name_lineA;
|
|
std::string name_lineB;
|
|
sl::float3 color;
|
|
};
|
|
|
|
// This class manages input events, window and Opengl rendering pipeline
|
|
|
|
class GLViewer {
|
|
public:
|
|
GLViewer();
|
|
~GLViewer();
|
|
bool isAvailable();
|
|
|
|
void init(int argc, char **argv);
|
|
|
|
void updateCamera(int, sl::Mat &, sl::Mat &);
|
|
void updateCamera(sl::Mat &);
|
|
|
|
void updateBodies(sl::Bodies &objs,std::map<sl::CameraIdentifier, sl::Bodies>& singledata, sl::FusionMetrics& metrics);
|
|
|
|
void setCameraPose(int, sl::Transform);
|
|
|
|
int getKey() {
|
|
const int key = last_key;
|
|
last_key = -1;
|
|
return key;
|
|
}
|
|
|
|
void exit();
|
|
private:
|
|
void render();
|
|
void update();
|
|
void draw();
|
|
void clearInputs();
|
|
void setRenderCameraProjection(sl::CameraParameters params, float znear, float zfar);
|
|
|
|
void printText();
|
|
|
|
// 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();
|
|
|
|
sl::float3 getColor(int, bool);
|
|
|
|
bool available;
|
|
bool drawBbox = false;
|
|
|
|
enum MOUSE_BUTTON {
|
|
LEFT = 0,
|
|
MIDDLE = 1,
|
|
RIGHT = 2,
|
|
WHEEL_UP = 3,
|
|
WHEEL_DOWN = 4
|
|
};
|
|
|
|
enum KEY_STATE {
|
|
UP = 'u',
|
|
DOWN = 'd',
|
|
FREE = 'f'
|
|
};
|
|
|
|
unsigned char lastPressedKey;
|
|
|
|
bool mouseButton_[3];
|
|
int mouseWheelPosition_;
|
|
int mouseCurrentPosition_[2];
|
|
int mouseMotion_[2];
|
|
int previousMouseMotion_[2];
|
|
KEY_STATE keyStates_[256];
|
|
|
|
std::mutex mtx;
|
|
std::mutex mtx_clr;
|
|
|
|
ShaderData shader;
|
|
|
|
sl::Transform projection_;
|
|
sl::float3 bckgrnd_clr;
|
|
|
|
std::map<int, PointCloud> point_clouds;
|
|
std::map<int, CameraViewer> viewers;
|
|
std::map<int, sl::Transform> poses;
|
|
|
|
std::map<int, Simple3DObject> skeletons_raw;
|
|
std::map<int, sl::float3> colors;
|
|
std::map<int, sl::float3> colors_sk;
|
|
|
|
std::vector<ObjectClassName> fusionStats;
|
|
|
|
CameraGL camera_;
|
|
Simple3DObject skeletons;
|
|
Simple3DObject floor_grid;
|
|
|
|
bool show_pc = true;
|
|
bool show_raw = false;
|
|
bool draw_flat_color = false;
|
|
|
|
std::uniform_int_distribution<uint16_t> uint_dist360;
|
|
std::mt19937 rng;
|
|
|
|
int last_key = -1;
|
|
};
|
|
|
|
#endif /* __VIEWER_INCLUDE__ */
|