奥比中光深度摄像头实现follow

Github地址:https://github.com/robotBing/skeleton_follow

演示视频地址:http://blog.jiutucao.com:9000/upload/2018/07/skeleton_follow.mp4

  奥比中光摄像头是有ros驱动的,最开始的点云颜色跟随就是直接调用奥比提供的astra_launch包,但是骨骼跟随自己需要做的工作就多得多了。不得不说过程很艰难,使用一门不是最擅长的语言,没有ide,只能用记事本进行代码编写,调试全靠打印信息。时间又紧,只能做出来一个半成品。

  下面说一下开发时的心路历程,先到astra的官网上下载奥比中光的sdk,这个sdk是可以进行骨骼识别的,但是和ros无关,我要做的就是根据提供demo代码,筛选出我需要的数据,对数据进行处理,映射成为ros需要的速度数据,最后将速度数据发布到cmd_vel话题上。最烦的就是没有ide,调用哪个方法要一点一点的试,全靠手打。

  Sdk提供的骨骼识别有19个关节信息,核心程序的思路就是提取身体最中心的关节坐标,映射为速度信息,提取手臂各个关节的坐标,对人的动作进行判断,让机器人在等待15s,开始跟随,停止跟随几个状态转换。程序有两个可以完善的地方1.程序没有使用多线程,等待15s过程中,整个线程停止,导致整个程序停止.2.动作的判断不够全面,受环境干扰太大。两个问题都是致命的,但是修改起来很简单,我就不做改进了,希望学弟们能把程序完善。

sdk下载地址:https://orbbec3d.com/develop

sdk中的demo编译需要sfml支持,下载sfml:

sudo apt-get install libsfml-dev

 

sfml官网教程地址:https://www.sfml-dev.org/tutorials/2.0/start-linux.php

下载sdk后编译例子步骤:

  1. 进入SDK下的samples目录下,新建一个build目录,在终端中进入到build目录下。

cd SDK路径/samples/

mkdir build

cd build

 

  1. 用cmake编译并转到bin目录下,运行测试程序。

cmake ..

make

编译后的可执行文件在build目录下的bin目录里

  下面是核心代码:

#include <SFML/Graphics.hpp>

#include <astra/astra.hpp>

#include <iostream>

#include <cstring>

#include "ros/ros.h"

#include "std_msgs/String.h"

#include <geometry_msgs/Twist.h>

#include <string>

#include <ctime>

#include <sstream>

class sfLine : public sf::Drawable

{

public:

    sfLine(const sf::Vector2f& point1, const sf::Vector2f& point2, sf::Color color, float thickness)

        : color_(color)

    {

        const sf::Vector2f direction = point2 - point1;

        const sf::Vector2f unitDirection = direction / std::sqrt(direction.x*direction.x + direction.y*direction.y);

        const sf::Vector2f normal(-unitDirection.y, unitDirection.x);

        const sf::Vector2f offset = (thickness / 2.f) * normal;

        vertices_[0].position = point1 + offset;

        vertices_[1].position = point2 + offset;

        vertices_[2].position = point2 - offset;

        vertices_[3].position = point1 - offset;

 

        for (int i = 0; i<4; ++i)

            vertices_[i].color = color;

    }

 

    void draw(sf::RenderTarget &target, sf::RenderStates states) const

    {

        target.draw(vertices_, 4, sf::Quads, states);

    }

private:

    sf::Vertex vertices_[4];

    sf::Color color_;

};

class BodyVisualizer : public astra::FrameListener

{

public:

      float line = 0;

      float angle = 0;

      ros::Publisher pub;

      ros::Publisher pubVoice;

      astra::Vector3f shoulderSpinePosition;

      astra::Vector3f leftHandPosition;

      astra::Vector3f rightHandPosition;

      int state = 0 ;

      int timeState = 1;

      virtual void onInit()

      {

      ros::NodeHandle n ;

      pub = n.advertise<geometry_msgs::Twist>("cmd_vel",1);

      pubVoice = n.advertise<std_msgs::String>("speak_string",1);

      }

    static sf::Color get_body_color(std::uint8_t bodyId)

    {

        if (bodyId == 0)

        {

            return sf::Color(0x00, 0x00, 0x00, 0x00);

        }

        // Case 0 below could mean bodyId == 25 or

        // above due to the "% 24".

        switch (bodyId % 24) {

        case 0:

            return sf::Color(0x00, 0x88, 0x00, 0xFF);

        case 1:

            return sf::Color(0x00, 0x00, 0xFF, 0xFF);

        case 2:

            return sf::Color(0x88, 0x00, 0x00, 0xFF);

        case 3:

            return sf::Color(0x00, 0xFF, 0x00, 0xFF);

        case 4:

            return sf::Color(0x00, 0x00, 0x88, 0xFF);

        case 5:

            return sf::Color(0xFF, 0x00, 0x00, 0xFF);

 

        case 6:

            return sf::Color(0xFF, 0x88, 0x00, 0xFF);

        case 7:

            return sf::Color(0xFF, 0x00, 0xFF, 0xFF);

        case 8:

            return sf::Color(0x88, 0x00, 0xFF, 0xFF);

        case 9:

            return sf::Color(0x00, 0xFF, 0xFF, 0xFF);

        case 10:

            return sf::Color(0x00, 0xFF, 0x88, 0xFF);

        case 11:

            return sf::Color(0xFF, 0xFF, 0x00, 0xFF);

 

        case 12:

            return sf::Color(0x00, 0x88, 0x88, 0xFF);

        case 13:

            return sf::Color(0x00, 0x88, 0xFF, 0xFF);

        case 14:

            return sf::Color(0x88, 0x88, 0x00, 0xFF);

        case 15:

            return sf::Color(0x88, 0xFF, 0x00, 0xFF);

        case 16:

            return sf::Color(0x88, 0x00, 0x88, 0xFF);

        case 17:

            return sf::Color(0xFF, 0x00, 0x88, 0xFF);

 

        case 18:

            return sf::Color(0xFF, 0x88, 0x88, 0xFF);

        case 19:

            return sf::Color(0xFF, 0x88, 0xFF, 0xFF);

        case 20:

            return sf::Color(0x88, 0x88, 0xFF, 0xFF);

        case 21:

            return sf::Color(0x88, 0xFF, 0xFF, 0xFF);

        case 22:

            return sf::Color(0x88, 0xFF, 0x88, 0xFF);

        case 23:

            return sf::Color(0xFF, 0xFF, 0x88, 0xFF);

        default:

            return sf::Color(0xAA, 0xAA, 0xAA, 0xFF);

        }

    }

    void init_depth_texture(int width, int height)

    {

        if (displayBuffer_ == nullptr || width != depthWidth_ || height != depthHeight_)

        {

            depthWidth_ = width;

            depthHeight_ = height;

            int byteLength = depthWidth_ * depthHeight_ * 4;

            displayBuffer_ = BufferPtr(new uint8_t[byteLength]);

            std::memset(displayBuffer_.get(), 0, byteLength);

            texture_.create(depthWidth_, depthHeight_);

            sprite_.setTexture(texture_, true);

            sprite_.setPosition(0, 0);

        }

    }

    void init_overlay_texture(int width, int height)

    {

        if (overlayBuffer_ == nullptr || width != overlayWidth_ || height != overlayHeight_)

        {

            overlayWidth_ = width;

            overlayHeight_ = height;

            int byteLength = overlayWidth_ * overlayHeight_ * 4;

            overlayBuffer_ = BufferPtr(new uint8_t[byteLength]);

            std::fill(&overlayBuffer_[0], &overlayBuffer_[0] + byteLength, 0);

            overlayTexture_.create(overlayWidth_, overlayHeight_);

            overlaySprite_.setTexture(overlayTexture_, true);

            overlaySprite_.setPosition(0, 0);

        }

    }

    void check_fps()

    {

        double fpsFactor = 0.02;

        std::clock_t newTimepoint= std::clock();

        long double frameDuration = (newTimepoint - lastTimepoint_) / static_cast<long double>(CLOCKS_PER_SEC);

        frameDuration_ = frameDuration * fpsFactor + frameDuration_ * (1 - fpsFactor);

        lastTimepoint_ = newTimepoint;

        double fps = 1.0 / frameDuration_;

 

       // printf("FPS: %3.1f (%3.4Lf ms)\n", fps, frameDuration_ * 1000);

    }

    void processDepth(astra::Frame& frame)

    {

        const astra::DepthFrame depthFrame = frame.get<astra::DepthFrame>();

        if (!depthFrame.is_valid()) { return; }

        int width = depthFrame.width();

        int height = depthFrame.height();

        init_depth_texture(width, height);

        const int16_t* depthPtr = depthFrame.data();

        for(int y = 0; y < height; y++)

        {

            for(int x = 0; x < width; x++)

            {

                int index = (x + y * width);

                int index4 = index * 4;

                int16_t depth = depthPtr[index];

                uint8_t value = depth % 255;

                displayBuffer_[index4] = value;

                displayBuffer_[index4 + 1] = value;

                displayBuffer_[index4 + 2] = value;

                displayBuffer_[index4 + 3] = 255;

            }

        }

        texture_.update(displayBuffer_.get());

    }

    void processBodies(astra::Frame& frame)

    {

        astra::BodyFrame bodyFrame = frame.get<astra::BodyFrame>();

        jointPositions_.clear();

        circles_.clear();

        circleShadows_.clear();

        boneLines_.clear();

        boneShadows_.clear();

        if (!bodyFrame.is_valid() || bodyFrame.info().width() == 0 || bodyFrame.info().height() == 0)

        {

            clear_overlay();

            return;

        }

        const float jointScale = bodyFrame.info().width() / 120.f;

       const auto& bodies = bodyFrame.bodies();

        for (auto& body : bodies)

        {

             //   bodyFrame.frame_index(), body.id(), unsigned(body.hand_poses().left_hand()));

            for(auto& joint : body.joints())

            {

                 onInit();

 

                 astra::JointType jointType = joint.type();

 

                 if(jointType == astra::JointType::ShoulderSpine)

                 {

                     shoulderSpinePosition = joint.world_position();

                    

 

                 }

                 if(jointType == astra::JointType::LeftHand)

                 {

                     leftHandPosition = joint.world_position();

                    

                 }

                 if(jointType == astra::JointType::RightHand)

                 {

                     rightHandPosition = joint.world_position();

                 }

                

                  //set state

                 if(leftHandPosition.y - shoulderSpinePosition.y > 300)

                  {

                     timeState++;

                     std_msgs::String msg;

 

                     std::string ss;

                     ss = "状态变为一,开始跟随" ;

                     msg.data = ss;                                 

                            pubVoice.publish(msg);

                     

                       state = 1 ;

                  }

                 if(rightHandPosition.y - shoulderSpinePosition.y > 300)

                  {

                     timeState++ ;

                     std_msgs::String msg;

 

                     std::string ss;

                     ss = "状态变为二,等待十五秒" ;

                     msg.data = ss;

                     pubVoice.publish(msg);               

                     state = 2 ;

                  }

                 if(leftHandPosition.y>-100 && leftHandPosition.y<100 && rightHandPosition.y<100 && rightHandPosition.y>-100)

                  {

                     timeState++ ;

                     std_msgs::String msg;

 

                     std::string ss;

                     ss = "状态变为零,停止跟随" ;

                     msg.data = ss;

                    

                    if(timeState % 100 == 0)

                       {

                            pubVoice.publish(msg);

                            state = 0 ;

                       }  

                  }

 

                 if(jointType == astra::JointType::MidSpine)

                 {

                    auto worldPosition = joint.world_position();

                    if(worldPosition.x / 500.f>0.2 || worldPosition.x / 500.f<-0.2)

                   {

                      if(worldPosition.x / 500.f>0.3)

                          {

                            angle = 0.3;

                          }else if(worldPosition.x / 500.f<-0.3)

                          {

                            angle = -0.3;

                          }else

                          {

                            angle = worldPosition.x / 500.f ;

                          }

                     

                   } else

                   {

                       if(worldPosition.x / 1000.f>0.3)

                          {

                            angle = 0.3;

                          }else if(worldPosition.x / 1000.f<-0.3)

                          {

                            angle = -0.3;

                          }else

                          {

                            angle = worldPosition.x / 1000.f ;

                          }

                   }

                  if((worldPosition.z - 1500.f) / 500.f > 0.2 ||  (worldPosition.z - 1500.f) / 500.f<-0.2)

                   {

                       if((worldPosition.z -1500.f) / 500.f>0.3)

                          {

                            line = 0.3;

                          }else if((worldPosition.z -1500.f) / 500.f<-0.3)

                          {

                            line = -0.3;

                          }else

                          {

                            line = (worldPosition.z -1500.f) / 500.f ;

                          }

                   }else

                   {

                       if((worldPosition.z -1500.f) / 1000.f>0.3)

                          {

                            line = 0.3;

                          }else if((worldPosition.z -1500.f) / 1000.f<-0.3)

                          {

                            line = -0.3;

                          }else

                          {

                            line = (worldPosition.z -1500.f) / 1000.f ;

                          }

                   }

 

                    publishCmdVel();

                    printf("z:%d\n",state);

                 }

                jointPositions_.push_back(joint.depth_position());

            }

 

            update_body(body, jointScale);

        }

 

        const auto& floor = bodyFrame.floor_info(); //floor

        if (floor.floor_detected())

        {

            const auto& p = floor.floor_plane();

            std::cout << "Floor plane: ["

                << p.a() << ", " << p.b() << ", " << p.c() << ", " << p.d()

                << "]" << std::endl;

 

        }

 

        const auto& bodyMask = bodyFrame.body_mask();

        const auto& floorMask = floor.floor_mask();

 

        update_overlay(bodyMask, floorMask);

    }

 

    void update_body(astra::Body body,

                     const float jointScale)

    {

        const auto& joints = body.joints();

 

        if (joints.empty())

        {

            return;

        }

 

        for (const auto& joint : joints)

        {

            astra::JointType type = joint.type();

            const auto& pos = joint.depth_position();

 

            if (joint.status() == astra::JointStatus::NotTracked)

            {

                continue;

            }

 

            auto radius = jointRadius_ * jointScale; // pixels

            sf::Color circleShadowColor(0, 0, 0, 255);

 

             auto color = sf::Color(0x00, 0xFF, 0x00, 0xFF);

 

            if ((type == astra::JointType::LeftHand && astra::HandPose::Grip==body.hand_poses().left_hand()) ||

                (type == astra::JointType::RightHand &&  astra::HandPose::Grip==body.hand_poses().right_hand()))

            {

                radius *= 1.5f;

                circleShadowColor = sf::Color(255, 255, 255, 255);

                color = sf::Color(0x00, 0xAA, 0xFF, 0xFF);

            }

 

            const auto shadowRadius = radius + shadowRadius_ * jointScale;

            const auto radiusDelta = shadowRadius - radius;

 

            sf::CircleShape circle(radius);

 

            circle.setFillColor(sf::Color(color.r, color.g, color.b, 255));

            circle.setPosition(pos.x - radius, pos.y - radius);

            circles_.push_back(circle);

 

            sf::CircleShape shadow(shadowRadius);

            shadow.setFillColor(circleShadowColor);

            shadow.setPosition(circle.getPosition() - sf::Vector2f(radiusDelta, radiusDelta));

            circleShadows_.push_back(shadow);

        }

 

        update_bone(joints, jointScale, astra::JointType::Head, astra::JointType::ShoulderSpine);

 

        update_bone(joints, jointScale, astra::JointType::ShoulderSpine, astra::JointType::LeftShoulder);

        update_bone(joints, jointScale, astra::JointType::LeftShoulder, astra::JointType::LeftElbow);

        update_bone(joints, jointScale, astra::JointType::LeftElbow, astra::JointType::LeftHand);

 

        update_bone(joints, jointScale, astra::JointType::ShoulderSpine, astra::JointType::RightShoulder);

        update_bone(joints, jointScale, astra::JointType::RightShoulder, astra::JointType::RightElbow);

        update_bone(joints, jointScale, astra::JointType::RightElbow, astra::JointType::RightHand);

 

        update_bone(joints, jointScale, astra::JointType::ShoulderSpine, astra::JointType::MidSpine);

        update_bone(joints, jointScale, astra::JointType::MidSpine, astra::JointType::BaseSpine);

 

        update_bone(joints, jointScale, astra::JointType::BaseSpine, astra::JointType::LeftHip);

        update_bone(joints, jointScale, astra::JointType::LeftHip, astra::JointType::LeftKnee);

        update_bone(joints, jointScale, astra::JointType::LeftKnee, astra::JointType::LeftFoot);

 

        update_bone(joints, jointScale, astra::JointType::BaseSpine, astra::JointType::RightHip);

        update_bone(joints, jointScale, astra::JointType::RightHip, astra::JointType::RightKnee);

        update_bone(joints, jointScale, astra::JointType::RightKnee, astra::JointType::RightFoot);

    }

 

    void update_bone(const astra::JointList& joints,

                     const float jointScale,astra::JointType j1,

                     astra::JointType j2)

    {

        const auto& joint1 = joints[int(j1)];

        const auto& joint2 = joints[int(j2)];

 

        if (joint1.status() == astra::JointStatus::NotTracked ||

            joint2.status() == astra::JointStatus::NotTracked)

        {

            return;

        }

        const auto& jp1 = joint1.depth_position();

        const auto& jp2 = joint2.depth_position();

 

        auto p1 = sf::Vector2f(jp1.x, jp1.y);

        auto p2 = sf::Vector2f(jp2.x, jp2.y);

 

        sf::Color color(255, 255, 255, 255);

        float thickness = lineThickness_ * jointScale;

        if (joint1.status() == astra::JointStatus::LowConfidence ||

            joint2.status() == astra::JointStatus::LowConfidence)

        {

            color = sf::Color(128, 128, 128, 255);

            thickness *= 0.5f;

        }

 

        boneLines_.push_back(sfLine(p1,

            p2,

            color,

            thickness));

        const float shadowLineThickness = thickness + shadowRadius_ * jointScale * 2.f;

        boneShadows_.push_back(sfLine(p1,

            p2,

            sf::Color(0, 0, 0, 255),

            shadowLineThickness));

    }

 

    void update_overlay(const astra::BodyMask& bodyMask,

                        const astra::FloorMask& floorMask)

    {

        const auto* bodyData = bodyMask.data();

        const auto* floorData = floorMask.data();

        const int width = bodyMask.width();

        const int height = bodyMask.height();

 

        init_overlay_texture(width, height);

 

        const int length = width * height;

 

        for (int i = 0; i < length; i++)

        {

            const auto bodyId = bodyData[i];

            const auto isFloor = floorData[i];

 

            sf::Color color(0x0, 0x0, 0x0, 0x0);

 

            if (bodyId != 0)

            {

                color = get_body_color(bodyId);

            }

            else if (isFloor != 0)

            {

                color = sf::Color(0x0, 0x0, 0xFF, 0x88);

            }

 

            const int rgbaOffset = i * 4;

            overlayBuffer_[rgbaOffset] = color.r;

            overlayBuffer_[rgbaOffset + 1] = color.g;

            overlayBuffer_[rgbaOffset + 2] = color.b;

            overlayBuffer_[rgbaOffset + 3] = color.a;

        }

 

        overlayTexture_.update(overlayBuffer_.get());

    }

 

    void clear_overlay()

    {

        int byteLength = overlayWidth_ * overlayHeight_ * 4;

        std::fill(&overlayBuffer_[0], &overlayBuffer_[0] + byteLength, 0);

 

        overlayTexture_.update(overlayBuffer_.get());

    }

 

    virtual void on_frame_ready(astra::StreamReader& reader,

                                astra::Frame& frame) override

    {

        processDepth(frame);

        processBodies(frame);

 

        check_fps();

    }

 

    void draw_bodies(sf::RenderWindow& window)

    {

        const float scaleX = window.getView().getSize().x / overlayWidth_;

        const float scaleY = window.getView().getSize().y / overlayHeight_;

 

        sf::RenderStates states;

        sf::Transform transform;

        transform.scale(scaleX, scaleY);

        states.transform *= transform;

 

        for (const auto& bone : boneShadows_)

            window.draw(bone, states);

 

        for (const auto& c : circleShadows_)

            window.draw(c, states);

 

        for (const auto& bone : boneLines_)

            window.draw(bone, states);

 

        for (auto& c : circles_)

            window.draw(c, states);

 

    }

 

    void draw_to(sf::RenderWindow& window)

    {

        if (displayBuffer_ != nullptr)

        {

            const float scaleX = window.getView().getSize().x / depthWidth_;

            const float scaleY = window.getView().getSize().y / depthHeight_;

            sprite_.setScale(scaleX, scaleY);

 

            window.draw(sprite_); // depth

        }

 

        if (overlayBuffer_ != nullptr)

        {

            const float scaleX = window.getView().getSize().x / overlayWidth_;

            const float scaleY = window.getView().getSize().y / overlayHeight_;

            overlaySprite_.setScale(scaleX, scaleY);

            window.draw(overlaySprite_); //bodymask and floormask

        }

 

        draw_bodies(window);

    }

    void publishCmdVel()

   {

     geometry_msgs::Twist twist;

     if(state == 1)

     {

     twist.linear.x = line;

 

     twist.angular.z = angle;

     pub.publish(twist);

     } else if(state == 2)

     {

        twist.linear.x = 0;

        twist.angular.z = 0;

        pub.publish(twist);

       clock_t delay;

       delay = 90 * 100000 ;

       clock_t start = clock();

       while(clock()-start < delay){

                state = 1;

              }

                    std_msgs::String msg;

 

                     std::string ss;

                     ss = "状态变为一,开始跟随" ;

                     msg.data = ss;

                   

                     pubVoice.publish(msg);

                   

      

     }else

     {

      twist.linear.x = 0;

     twist.angular.z = 0;

     pub.publish(twist);

     }

 

 

    

   }

 

private:

    long double frameDuration_{ 0 };

    std::clock_t lastTimepoint_ { 0 };

    sf::Texture texture_;

    sf::Sprite sprite_;

    using BufferPtr = std::unique_ptr < uint8_t[] >;

    BufferPtr displayBuffer_{ nullptr };

    std::vector<astra::Vector2f> jointPositions_;

    int depthWidth_{0};

    int depthHeight_{0};

    int overlayWidth_{0};

    int overlayHeight_{0};

    std::vector<sfLine> boneLines_;

    std::vector<sfLine> boneShadows_;

    std::vector<sf::CircleShape> circles_;

    std::vector<sf::CircleShape> circleShadows_;

    float lineThickness_{ 0.5f }; // pixels

    float jointRadius_{ 1.0f };   // pixels

    float shadowRadius_{ 0.5f };  // pixels

    BufferPtr overlayBuffer_{ nullptr };

    sf::Texture overlayTexture_;

    sf::Sprite overlaySprite_;

 

};

 

astra::DepthStream configure_depth(astra::StreamReader& reader)

{

    auto depthStream = reader.stream<astra::DepthStream>();

    astra::ImageStreamMode depthMode;

    depthMode.set_width(640);

    depthMode.set_height(480);

    depthMode.set_pixel_format(astra_pixel_formats::ASTRA_PIXEL_FORMAT_DEPTH_MM);

    depthMode.set_fps(30);

    depthStream.set_mode(depthMode);

    return depthStream;

}

 

int main(int argc, char** argv)

{

     ros::init(argc , argv ,"skeleton_follow");

    astra::initialize();

    const char* licenseString = "<INSERT LICENSE KEY HERE>";

    orbbec_body_tracking_set_license(licenseString);

    sf::RenderWindow window(sf::VideoMode(1280, 960), "Simple Body Viewer");

#ifdef _WIN32

    auto fullscreenStyle = sf::Style::None;

#else

    auto fullscreenStyle = sf::Style::Fullscreen;

#endif

 

    const sf::VideoMode fullScreenMode = sf::VideoMode::getFullscreenModes()[0];

    const sf::VideoMode windowedMode(1280, 960);

    bool isFullScreen = false;

 

    astra::StreamSet sensor;

    astra::StreamReader reader = sensor.create_reader();

    BodyVisualizer listener;

    auto depthStream = configure_depth(reader);

    depthStream.start();

    reader.stream<astra::BodyStream>().start();

    reader.add_listener(listener);

    BodyVisualizer bv ;

 

    while (window.isOpen())

    {

        astra_update();

 

        sf::Event event;

 

        while (window.pollEvent(event))

        {

            switch (event.type)

            {

            case sf::Event::Closed:

                window.close();

                break;

            case sf::Event::KeyPressed:

            {

                if (event.key.code == sf::Keyboard::C && event.key.control)

                {

                    window.close();

                }

                switch (event.key.code)

                {

                case sf::Keyboard::Escape:

                    window.close();

                    break;

                case sf::Keyboard::F:

                    if (isFullScreen)

                    {

                        window.create(windowedMode, "Simple Body Viewer", sf::Style::Default);

                    }

                    else

                    {

                        window.create(fullScreenMode, "Simple Body Viewer", fullscreenStyle);

                    }

                    isFullScreen = !isFullScreen;

                    break;

                case sf::Keyboard::R:

                    depthStream.enable_registration(!depthStream.registration_enabled());

                    break;

                case sf::Keyboard::M:

                    depthStream.enable_mirroring(!depthStream.mirroring_enabled());

                    break;

                default:

                    break;

                }

                break;

            }

            default:

                break;

            }

        }

        // clear the window with black color

        window.clear(sf::Color::Black);

        listener.draw_to(window);

        window.display();

    }

    astra::terminate();

    return 0;

}

已标记关键词 清除标记
相关推荐
©️2020 CSDN 皮肤主题: 大白 设计师:CSDN官方博客 返回首页