x

include/ClientPublisher.hpp

@@ -0,0 +1,74 @@
+#ifndef  __SENDER_RUNNER_HDR__
+#define __SENDER_RUNNER_HDR__
+
+#include <sl/Camera.hpp>
+#include <sl/Fusion.hpp>
+
+#include <thread>
+#include <condition_variable>
+
+// Hard-coded camera stream configuration from inside_network.json
+struct CameraStreamConfig {
+    int serial_number;
+    std::string ip_address;
+    int port;
+};
+
+// Predefined camera stream configurations
+const std::vector<CameraStreamConfig> CAMERA_STREAM_CONFIGS = {
+    {44289123, "192.168.128.2", 30000},
+    {44435674, "192.168.128.2", 30002},
+    {41831756, "192.168.128.2", 30004},
+    {46195029, "192.168.128.2", 30006}
+};
+
+struct Trigger{
+
+    void notifyZED(){
+
+        cv.notify_all();
+
+        if(running){
+            bool wait_for_zed = true;
+            const int nb_zed = states.size();
+            while(wait_for_zed){
+                int count_r = 0;
+                for(auto &it:states)
+                    count_r += it.second;
+                wait_for_zed = count_r != nb_zed;
+                sl::sleep_ms(1);
+            }
+            for(auto &it:states)
+                it.second = false;
+        }
+    }
+
+    std::condition_variable cv;
+    bool running = true;
+    std::map<int, bool> states;
+};
+
+class ClientPublisher{
+
+public:
+    ClientPublisher();
+    ~ClientPublisher();
+
+    // Open camera from stream (IP:port) instead of local input
+    bool open(const std::string& ip, int port, Trigger* ref, int sdk_gpu_id);
+    void start();
+    void stop();
+    void setStartSVOPosition(unsigned pos);
+
+    int getSerialNumber() const { return serial; }
+
+private:
+    sl::Camera zed;
+    void work();
+    std::thread runner;
+    int serial;
+    std::mutex mtx;
+    Trigger *p_trigger;
+};
+
+#endif // ! __SENDER_RUNNER_HDR__

include/GLViewer.hpp

@@ -0,0 +1,336 @@
+#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__ */

include/utils.hpp

@@ -0,0 +1,56 @@
+#pragma once
+
+#include <sl/Camera.hpp>
+
+/**
+* @brief Compute the start frame of each SVO for playback to be synced
+*
+* @param svo_files Map camera index to SVO file path
+* @return Map camera index to starting SVO frame for synced playback
+*/
+std::map<int, int> syncDATA(std::map<int, std::string> svo_files) {
+    std::map<int, int> output; // map of camera index and frame index of the starting point for each
+
+    // Open all SVO
+    std::map<int, std::shared_ptr<sl::Camera>> p_zeds;
+
+    for (auto &it : svo_files) {
+        auto p_zed = std::make_shared<sl::Camera>();
+
+        sl::InitParameters init_param;
+        init_param.depth_mode = sl::DEPTH_MODE::NONE;
+        init_param.camera_disable_self_calib = true;
+        init_param.input.setFromSVOFile(it.second.c_str());
+
+        auto error = p_zed->open(init_param);
+        if (error == sl::ERROR_CODE::SUCCESS)
+            p_zeds.insert(std::make_pair(it.first, p_zed));
+        else {
+            std::cerr << "Could not open file " << it.second.c_str() << ": " << sl::toString(error) << ". Skipping" << std::endl;
+        }
+    }
+
+    // Compute the starting point, we have to take the latest one
+    sl::Timestamp start_ts = 0;
+    for (auto &it : p_zeds) {
+        it.second->grab();
+        auto ts = it.second->getTimestamp(sl::TIME_REFERENCE::IMAGE);
+
+        if (ts > start_ts)
+            start_ts = ts;
+    }
+
+    std::cout << "Found SVOs common starting time: " << start_ts << std::endl;
+
+    // The starting point is now known, let's find the frame idx for all corresponding
+    for (auto &it : p_zeds) {
+        auto frame_position_at_ts = it.second->getSVOPositionAtTimestamp(start_ts);
+
+        if (frame_position_at_ts != -1)
+            output.insert(std::make_pair(it.first, frame_position_at_ts));
+    }
+
+    for (auto &it : p_zeds) it.second->close();
+
+    return output;
+}