ocl_processing.hpp

Go to the documentation of this file.

#ifndef OCL_PROCESSING_HPP
#define OCL_PROCESSING_HPP

#include <functional>
#include <mutex>
#include <GL/glew.h>  // Add before CLUtils.hpp
#include <CLUtils.hpp>
#include <GuidedFilter/algorithms.hpp>
#include <ICP/algorithms.hpp>
#include <eigen3/Eigen/Dense>
#include <freenect_rgbd.hpp>
#include <octomap/octomap.h>
#include <octomap/OcTree.h>
// #include <octomap/ColorOcTree.h>
#include <oclslam/pointcloud.hpp>
#include <oclslam/algorithms.hpp>

using namespace cl_algo;


class CLEnvGL : public clutils::CLEnv
{
public:
    CLEnvGL (int width, int height, int numPC);

private:
    void initGLMemObjects ();

    int width, height, numPC;

};


template <ICP::ICPStepConfigT CR, ICP::ICPStepConfigW CW>
class OCLSLAM
{
public:
    OCLSLAM (Kinect *kinect, octomap::OcTree &map);
    ~OCLSLAM ();
    void init ();
    void registerPointCloud ();
    void write (std::string filename = std::string ("map.ot"));
    void writeBinary (std::string filename = std::string ("map.bt"));
    void display ();
    bool getSLAMStatus () { return slamStatus; }
    void setSLAMStatus (bool flag);
    void toggleSLAMStatus ();
    bool getGFRGBStatus () { return gfRGBStatus; }
    void setGFRGBStatus (bool flag) { gfRGBStatus = flag; }
    void toggleGFRGBStatus () { gfRGBStatus = !gfRGBStatus; }
    bool getGFDStatus () { return gfDRadius; }
    void setGFDStatus (bool flag) { gfDStatus = flag; }
    void toggleGFDStatus () { gfDStatus = !gfDStatus; }
    int getRGBNormalization () { return rgbNorm; }
    void setRGBNormalization (int flag) { rgbNorm = flag; to8D.setRGBNorm (rgbNorm); }
    void toggleRGBNormalization () { rgbNorm = !rgbNorm; to8D.setRGBNorm (rgbNorm); }
    int getGFRGBRadius () { return gfRGB.getRadius (); }
    void setGFRGBRadius (int radius) { gfRGBRadius = radius; gfRGB.setRadius (radius); }
    float getGFRGBEps () { return gfRGB.getEps (); }
    void setGFRGBEps (float eps) { gfRGBEps = eps; gfRGB.setEps (eps); }
    int getGFDRadius () { return gfD.getRadius (); }
    void setGFDRadius (int radius) { gfDRadius = radius; gfD.setRadius (radius); }
    float getGFDEps () { return gfD.getEps (); }
    void setGFDEps (float eps) { gfDEps = eps; gfD.setEps (eps); }
    float getGFDScaling () { return gfD.getDScaling (); }
    void setGFDScaling (float scaling) { gfDScaling = scaling; gfD.setDScaling (scaling); }
    float getSensorFocalLength () { return to8D.getFocalLength (); }
    void setSensorFocalLength (float f) { focalLength = f; to8D.setFocalLength (f); }
    float getRBCAlpha () { return icp.getAlpha (); }
    void setRBCAlpha (float _a) { a = _a; icp.setAlpha (_a); }
    float getICPSScaling () { return icp.getScaling (); }
    void setICPSScaling (float _c) { c = _c; icp.setScaling (_c); }
    unsigned int getICPMaxIterations () { return icp.getMaxIterations (); }
    void setICPMaxIterations (unsigned int maxIter) { max_iterations = maxIter; icp.setMaxIterations (maxIter); }
    double getICPAngleThreshold () { return icp.getAngleThreshold (); }
    void setICPAngleThreshold (double at) { angle_threshold = at; icp.setAngleThreshold (at); }
    double getICPTranslationThreshold () { return icp.getTranslationThreshold (); }
    void setICPTranslationThreshold (double tt) { translation_threshold = tt; icp.setTranslationThreshold (tt); }

    std::function<void ()> slam;

    volatile int timeStep;  
    // Global localization parameters
    Eigen::Matrix3f R_g;     
    Eigen::Quaternionf q_g;  
    Eigen::Vector3f t_g;     
    cl_float s_g;            
private:
    void _mapping ();

    // Internal parameters
    int gfRGBRadius;
    float gfRGBEps;
    int gfDRadius;
    float gfDEps;
    float gfDScaling;
    float focalLength;
    float a;
    float c;
    unsigned int max_iterations;
    double angle_threshold;
    double translation_threshold;

    // External parameters
    volatile bool slamStatus;
    volatile bool gfRGBStatus;
    volatile bool gfDStatus;
    volatile int rgbNorm;

    size_t slamFuncHashCode;
    int maxPCGL;  // Limits in the number of point clouds held in memory for visualization
    unsigned int width, height;
    unsigned int n;  // Number of points in a point cloud
    unsigned int m;  // Number of landmarks
    unsigned int r;  // Number of representatives

    CLEnvGL env;
    clutils::CLEnvInfo<2> infoGF;
    clutils::CLEnvInfo<1> infoRBC, infoICP, infoSLAM;
    cl::Context &context;
    cl::CommandQueue &queue0, &queue1;
    
    Kinect *kinect;

    cl_float *hPtrTg;
    cl::Buffer hBufferTg;
    cl::Buffer hBufferRGB, hBufferD;
    cl::Buffer dBufferRGB, dBufferD;
    std::vector<cl::BufferGL> dBufferGL;

    GF::Kinect::GuidedFilterRGB<GF::Kinect::GuidedFilterRGBConfig::SEPARATED> gfRGB;
    GF::Kinect::GuidedFilterDepth gfD;
    GF::SeparateRGB<GF::SeparateRGBConfig::UCHAR_FLOAT> sepRGB;
    GF::Depth<GF::DepthConfig::USHORT_FLOAT> convD;
    GF::RGBDTo8D to8D;
    ICP::ICPLMs lm;
    ICP::ICP<CR, CW> icp;
    ICP::ICPTransform<ICP::ICPTransformConfig::QUATERNION> transform;
    GF::SplitPC8D sp8D;
    oclslam::SplitPC8D sp8DMap;

    cl::Event eventGL;
    std::vector<cl::Event> waitListGL;
    clutils::CPUTimer<double, std::milli> timer;
    clutils::CPUTimer<double, std::milli> timerICP;
    volatile double lICP;

    // Map parameters
    octomap::point3d global_pos;  // Global position in meters
    oclslam::PointCloud pc;
    // std::vector<octomap::ColorOcTreeNode::Color> cc;
    octomap::OcTree &map;
    // octomap::ColorOcTree &map;
};

#endif  // OCL_PROCESSING_HPP