c_cpp 使用继承Kinect2Grabber的CustomGrabber从Kinect v2中检索Body（Joint）

Posted 2021-05-14

#include "stdafx.h"

// Disable Error C4996 that occur when using Boost.Signals2.
#ifdef _DEBUG
    #define _SCL_SECURE_NO_WARNINGS
#endif

#include "kinect2_grabber.h"
#include <pcl/visualization/cloud_viewer.h>

namespace pcl{
    class CustomGrabber : public pcl::Kinect2Grabber
    {
        public:
            CustomGrabber()
                : source( nullptr )
                , reader( nullptr )
            {
                result = sensor->get_BodyFrameSource( &source );
                if( FAILED( result ) ){
                    throw std::exception( "Exception : IKinectSensor::get_BodyFrameSource()" );
                }

                result = source->OpenReader( &reader );
                if( FAILED( result ) ){
                    throw std::exception( "Exception : IBodyFrameSource::OpenReader()" );
                }

                thread = boost::thread( &CustomGrabber::threadFunction, this );
            };

            ~CustomGrabber()
            {
                SafeRelease( source );
                SafeRelease( reader );

                thread.join();
            };

            HRESULT GetJoints( unsigned int count, Joint* joints ){
                boost::mutex::scoped_lock( mutex );

                if( data[count].tracked ){
                    for( int type = 0; type < JointType::JointType_Count; type++ ){
                        joints[type] = data[count].joints[type];
                    }
                    return S_OK;
                }
                else{
                    return S_FALSE;
                }
            };

        private:
            IBodyFrameSource* source;
            IBodyFrameReader* reader;
            boost::thread thread;

            struct Data{
                Joint joints[JointType::JointType_Count];
                BOOLEAN tracked;
            };
            Data data[BODY_COUNT];

            void threadFunction()
            {
                while( !quit ){
                    boost::unique_lock<boost::mutex> lock( mutex );

                    IBodyFrame* frame = nullptr;
                    result = reader->AcquireLatestFrame( &frame );
                    if( SUCCEEDED( result ) ){
                        IBody* body[BODY_COUNT] = { 0 };
                        result = frame->GetAndRefreshBodyData( BODY_COUNT, body );
                        if( SUCCEEDED( result ) ){
                            for( int count = 0; count < BODY_COUNT; count++ ){
                                data[count].tracked = false;
                                result = body[count]->get_IsTracked( &data[count].tracked );
                                if( SUCCEEDED( result ) && data[count].tracked ){
                                    body[count]->GetJoints( JointType::JointType_Count, data[count].joints );
                                }
                            }
                        }
                        for( int count = 0; count < BODY_COUNT; count++ ){
                            SafeRelease( body[count] );
                        }
                    }
                    SafeRelease( frame );

                    lock.unlock();
                }
            };
    };
}

int _tmain( int argc, _TCHAR* argv[] )
{
    // Create Cloud Viewer
    pcl::visualization::CloudViewer viewer( "Point Cloud Viewer" );

    // Callback Function to be called when Updating Data
    boost::function<void( const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& )> function =
        [&viewer]( const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &cloud ){
        if( !viewer.wasStopped() ){
            viewer.showCloud( cloud );
        }
    };

    // Create CustomGrabber
    pcl::CustomGrabber* grabber = new pcl::CustomGrabber();
    HRESULT result = S_OK;

    // Regist Callback Function
    grabber->registerCallback( function );

    // Start Retrieve Data
    grabber->start();

    while( !viewer.wasStopped() ){
        // Get Other Data from CustomGrabber (e.g. Body)
        for( int count = 0; count < BODY_COUNT; count++ ){
            Joint joint[JointType::JointType_Count];
            result = grabber->GetJoints( count, joint );
            if( SUCCEEDED( result ) ){
                /*for( int type = 0; type < JointType::JointType_Count; type++ ){
                    CameraSpacePoint position = joint[type].Position;
                    float x = position.X;
                    float y = position.Y;
                    float z = position.Z;
                }*/
            }
        }

        // Input Key ( Exit ESC key )
        if( GetKeyState( VK_ESCAPE ) < 0 ){
            break;
        }
    }

    // Stop Retrieve Data
    grabber->stop();

    return 0;
}

namespace pcl{
	class CustomGrabber : public pcl::Kinect2Grabber
	{
		public:
			CustomGrabber()
				: source( nullptr )
				, reader( nullptr )
			{
				result = sensor->get_BodyFrameSource( &source );
				if( FAILED( result ) ){
					throw std::exception( "Exception : IKinectSensor::get_BodyFrameSource()" );
				}

				result = source->OpenReader( &reader );
				if( FAILED( result ) ){
					throw std::exception( "Exception : IBodyFrameSource::OpenReader()" );
				}

				thread = boost::thread( &CustomGrabber::threadFunction, this );
			};

			~CustomGrabber()
			{
				SafeRelease( source );
				SafeRelease( reader );

				thread.join();
			};

			HRESULT GetJoints( unsigned int count, Joint* joints ){
				boost::mutex::scoped_lock( mutex );

				if( data[count].tracked ){
					for( int type = 0; type < JointType::JointType_Count; type++ ){
						joints[type] = data[count].joints[type];
					}
					return S_OK;
				}
				else{
					return S_FALSE;
				}
			};

		private:
			IBodyFrameSource* source;
			IBodyFrameReader* reader;
			boost::thread thread;

			struct Data{
				Joint joints[JointType::JointType_Count];
				BOOLEAN tracked;
			};
			Data data[BODY_COUNT];

			void threadFunction()
			{
				while( !quit ){
					boost::unique_lock<boost::mutex> lock( mutex );

					IBodyFrame* frame = nullptr;
					result = reader->AcquireLatestFrame( &frame );
					if( SUCCEEDED( result ) ){
						IBody* body[BODY_COUNT] = { 0 };
						result = frame->GetAndRefreshBodyData( BODY_COUNT, body );
						if( SUCCEEDED( result ) ){
							for( int count = 0; count < BODY_COUNT; count++ ){
								data[count].tracked = false;
								result = body[count]->get_IsTracked( &data[count].tracked );
								if( SUCCEEDED( result ) && data[count].tracked ){
									body[count]->GetJoints( JointType::JointType_Count, data[count].joints );
								}
							}
						}
						for( int count = 0; count < BODY_COUNT; count++ ){
							SafeRelease( body[count] );
						}
					}
					SafeRelease( frame );

					lock.unlock();
				}
			};
	};
}