mirror of
https://github.com/storytold/LiveScan3D.git
synced 2026-10-09 00:09:58 +00:00
Working with Azure Kinect but without body and only single sensor per machine
This commit is contained in:
Marek Kowalski
+8
-8
@@ -1,5 +1,5 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<Project DefaultTargets="Build" ToolsVersion="14.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||
<Project DefaultTargets="Build" ToolsVersion="15.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||
<ItemGroup Label="ProjectConfigurations">
|
||||
<ProjectConfiguration Include="Debug|Win32">
|
||||
<Configuration>Debug</Configuration>
|
||||
@@ -29,46 +29,46 @@
|
||||
<PropertyGroup Label="Globals">
|
||||
<ProjectGuid>{973EE923-B423-4BCD-AA08-B03DA40CB51F}</ProjectGuid>
|
||||
<RootNamespace>ICP</RootNamespace>
|
||||
<WindowsTargetPlatformVersion>8.1</WindowsTargetPlatformVersion>
|
||||
<WindowsTargetPlatformVersion>10.0.17763.0</WindowsTargetPlatformVersion>
|
||||
</PropertyGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>true</UseDebugLibraries>
|
||||
<PlatformToolset>v140</PlatformToolset>
|
||||
<PlatformToolset>v141</PlatformToolset>
|
||||
<CharacterSet>MultiByte</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>true</UseDebugLibraries>
|
||||
<PlatformToolset>v140</PlatformToolset>
|
||||
<PlatformToolset>v141</PlatformToolset>
|
||||
<CharacterSet>MultiByte</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>false</UseDebugLibraries>
|
||||
<PlatformToolset>v140</PlatformToolset>
|
||||
<PlatformToolset>v141</PlatformToolset>
|
||||
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||
<CharacterSet>MultiByte</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>false</UseDebugLibraries>
|
||||
<PlatformToolset>v140</PlatformToolset>
|
||||
<PlatformToolset>v141</PlatformToolset>
|
||||
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||
<CharacterSet>MultiByte</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release DLL|Win32'" Label="Configuration">
|
||||
<ConfigurationType>DynamicLibrary</ConfigurationType>
|
||||
<UseDebugLibraries>false</UseDebugLibraries>
|
||||
<PlatformToolset>v140</PlatformToolset>
|
||||
<PlatformToolset>v141</PlatformToolset>
|
||||
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||
<CharacterSet>MultiByte</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release DLL|x64'" Label="Configuration">
|
||||
<ConfigurationType>DynamicLibrary</ConfigurationType>
|
||||
<UseDebugLibraries>false</UseDebugLibraries>
|
||||
<PlatformToolset>v140</PlatformToolset>
|
||||
<PlatformToolset>v141</PlatformToolset>
|
||||
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||
<CharacterSet>MultiByte</CharacterSet>
|
||||
</PropertyGroup>
|
||||
|
||||
@@ -1,5 +1,5 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<Project DefaultTargets="Build" ToolsVersion="14.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||
<Project DefaultTargets="Build" ToolsVersion="15.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||
<ItemGroup Label="ProjectConfigurations">
|
||||
<ProjectConfiguration Include="Debug|Win32">
|
||||
<Configuration>Debug</Configuration>
|
||||
@@ -19,13 +19,13 @@
|
||||
</ProjectConfiguration>
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ClInclude Include="..\include\LiveScanClient\azureKinectCapture.h" />
|
||||
<ClInclude Include="..\include\LiveScanClient\calibration.h" />
|
||||
<ClInclude Include="..\include\LiveScanClient\filter.h" />
|
||||
<ClInclude Include="..\include\LiveScanClient\frameFileWriterReader.h" />
|
||||
<ClInclude Include="..\include\LiveScanClient\iCapture.h" />
|
||||
<ClInclude Include="..\include\LiveScanClient\imageRenderer.h" />
|
||||
<ClInclude Include="..\include\LiveScanClient\iMarker.h" />
|
||||
<ClInclude Include="..\include\LiveScanClient\kinectCapture.h" />
|
||||
<ClInclude Include="..\include\LiveScanClient\liveScanClient.h" />
|
||||
<ClInclude Include="..\include\LiveScanClient\marker.h" />
|
||||
<ClInclude Include="..\include\LiveScanClient\utils.h" />
|
||||
@@ -35,13 +35,13 @@
|
||||
<ClInclude Include="stdafx.h" />
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ClCompile Include="..\src\LiveScanClient\azureKinectCapture.cpp" />
|
||||
<ClCompile Include="..\src\LiveScanClient\calibration.cpp" />
|
||||
<ClCompile Include="..\src\LiveScanClient\filter.cpp" />
|
||||
<ClCompile Include="..\src\LiveScanClient\frameFileWriterReader.cpp" />
|
||||
<ClCompile Include="..\src\LiveScanClient\iCapture.cpp" />
|
||||
<ClCompile Include="..\src\LiveScanClient\imageRenderer.cpp" />
|
||||
<ClCompile Include="..\src\LiveScanClient\iMarker.cpp" />
|
||||
<ClCompile Include="..\src\LiveScanClient\kinectCapture.cpp" />
|
||||
<ClCompile Include="..\src\LiveScanClient\liveScanClient.cpp" />
|
||||
<ClCompile Include="..\src\LiveScanClient\marker.cpp" />
|
||||
<ClCompile Include="..\src\LiveScanClient\utils.cpp" />
|
||||
@@ -53,36 +53,39 @@
|
||||
<ItemGroup>
|
||||
<Image Include="app.ico" />
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<None Include="packages.config" />
|
||||
</ItemGroup>
|
||||
<PropertyGroup Label="Globals">
|
||||
<ProjectGuid>{9B550BBA-EAFB-4D12-8B1C-8FDA39361F52}</ProjectGuid>
|
||||
<RootNamespace>KinectClient</RootNamespace>
|
||||
<ProjectName>LiveScanClient</ProjectName>
|
||||
<WindowsTargetPlatformVersion>8.1</WindowsTargetPlatformVersion>
|
||||
<WindowsTargetPlatformVersion>10.0.17763.0</WindowsTargetPlatformVersion>
|
||||
</PropertyGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>true</UseDebugLibraries>
|
||||
<PlatformToolset>v140</PlatformToolset>
|
||||
<PlatformToolset>v141</PlatformToolset>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>true</UseDebugLibraries>
|
||||
<PlatformToolset>v140</PlatformToolset>
|
||||
<PlatformToolset>v141</PlatformToolset>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>false</UseDebugLibraries>
|
||||
<PlatformToolset>v140</PlatformToolset>
|
||||
<PlatformToolset>v141</PlatformToolset>
|
||||
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>false</UseDebugLibraries>
|
||||
<PlatformToolset>v140</PlatformToolset>
|
||||
<PlatformToolset>v141</PlatformToolset>
|
||||
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
@@ -144,7 +147,7 @@
|
||||
<Link>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
<AdditionalLibraryDirectories>$(KINECTSDK20_DIR)\lib\x64;$(SolutionDir)lib</AdditionalLibraryDirectories>
|
||||
<AdditionalDependencies>opencv_world320d.lib;kinect20.lib;libzstd.lib;%(AdditionalDependencies)</AdditionalDependencies>
|
||||
<AdditionalDependencies>opencv_world320d.lib;libzstd.lib;%(AdditionalDependencies)</AdditionalDependencies>
|
||||
<SubSystem>NotSet</SubSystem>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
@@ -184,11 +187,18 @@
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||
<OptimizeReferences>true</OptimizeReferences>
|
||||
<AdditionalDependencies>opencv_world320.lib;kinect20.lib;libzstd.lib;%(AdditionalDependencies)</AdditionalDependencies>
|
||||
<AdditionalDependencies>opencv_world320.lib;libzstd.lib;%(AdditionalDependencies)</AdditionalDependencies>
|
||||
<AdditionalLibraryDirectories>$(KINECTSDK20_DIR)\lib\x64;$(SolutionDir)lib</AdditionalLibraryDirectories>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
|
||||
<ImportGroup Label="ExtensionTargets">
|
||||
<Import Project="..\packages\Microsoft.Azure.Kinect.Sensor.1.2.0-beta.1\build\native\Microsoft.Azure.Kinect.Sensor.targets" Condition="Exists('..\packages\Microsoft.Azure.Kinect.Sensor.1.2.0-beta.1\build\native\Microsoft.Azure.Kinect.Sensor.targets')" />
|
||||
</ImportGroup>
|
||||
<Target Name="EnsureNuGetPackageBuildImports" BeforeTargets="PrepareForBuild">
|
||||
<PropertyGroup>
|
||||
<ErrorText>This project references NuGet package(s) that are missing on this computer. Use NuGet Package Restore to download them. For more information, see http://go.microsoft.com/fwlink/?LinkID=322105. The missing file is {0}.</ErrorText>
|
||||
</PropertyGroup>
|
||||
<Error Condition="!Exists('..\packages\Microsoft.Azure.Kinect.Sensor.1.2.0-beta.1\build\native\Microsoft.Azure.Kinect.Sensor.targets')" Text="$([System.String]::Format('$(ErrorText)', '..\packages\Microsoft.Azure.Kinect.Sensor.1.2.0-beta.1\build\native\Microsoft.Azure.Kinect.Sensor.targets'))" />
|
||||
</Target>
|
||||
</Project>
|
||||
@@ -42,9 +42,6 @@
|
||||
<ClInclude Include="..\include\LiveScanClient\iMarker.h">
|
||||
<Filter>Header Files</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="..\include\LiveScanClient\kinectCapture.h">
|
||||
<Filter>Header Files</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="..\include\LiveScanClient\liveScanClient.h">
|
||||
<Filter>Header Files</Filter>
|
||||
</ClInclude>
|
||||
@@ -57,6 +54,9 @@
|
||||
<ClInclude Include="..\include\LiveScanClient\frameFileWriterReader.h">
|
||||
<Filter>Header Files</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="..\include\LiveScanClient\azureKinectCapture.h">
|
||||
<Filter>Header Files</Filter>
|
||||
</ClInclude>
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ClCompile Include="..\src\socketCS.cpp">
|
||||
@@ -77,9 +77,6 @@
|
||||
<ClCompile Include="..\src\LiveScanClient\iMarker.cpp">
|
||||
<Filter>Source Files</Filter>
|
||||
</ClCompile>
|
||||
<ClCompile Include="..\src\LiveScanClient\kinectCapture.cpp">
|
||||
<Filter>Source Files</Filter>
|
||||
</ClCompile>
|
||||
<ClCompile Include="..\src\LiveScanClient\liveScanClient.cpp">
|
||||
<Filter>Source Files</Filter>
|
||||
</ClCompile>
|
||||
@@ -92,6 +89,9 @@
|
||||
<ClCompile Include="..\src\LiveScanClient\frameFileWriterReader.cpp">
|
||||
<Filter>Source Files</Filter>
|
||||
</ClCompile>
|
||||
<ClCompile Include="..\src\LiveScanClient\azureKinectCapture.cpp">
|
||||
<Filter>Source Files</Filter>
|
||||
</ClCompile>
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<Image Include="app.ico">
|
||||
@@ -103,4 +103,7 @@
|
||||
<Filter>Resource Files</Filter>
|
||||
</ResourceCompile>
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<None Include="packages.config" />
|
||||
</ItemGroup>
|
||||
</Project>
|
||||
@@ -0,0 +1,4 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<packages>
|
||||
<package id="Microsoft.Azure.Kinect.Sensor" version="1.2.0-beta.1" targetFramework="native" />
|
||||
</packages>
|
||||
@@ -0,0 +1,33 @@
|
||||
#pragma once
|
||||
|
||||
#include "stdafx.h"
|
||||
#include "ICapture.h"
|
||||
#include <k4a/k4a.h>
|
||||
#include "utils.h"
|
||||
|
||||
class AzureKinectCapture : public ICapture
|
||||
{
|
||||
public:
|
||||
AzureKinectCapture();
|
||||
~AzureKinectCapture();
|
||||
|
||||
bool Initialize();
|
||||
bool Initialize(int deviceIdx);
|
||||
bool AcquireFrame();
|
||||
void MapDepthFrameToCameraSpace(Point3f *pCameraSpacePoints);
|
||||
void MapColorFrameToCameraSpace(Point3f *pCameraSpacePoints);
|
||||
void MapDepthFrameToColorSpace(UINT16 *pDepthInColorSpace);
|
||||
void MapColorFrameToDepthSpace(RGB *pColorInDepthSpace);
|
||||
private:
|
||||
k4a_device_t kinectSensor = NULL;
|
||||
int32_t captureTimeoutMs = 1000;
|
||||
|
||||
k4a_image_t colorImage = NULL;
|
||||
k4a_image_t depthImage = NULL;
|
||||
k4a_image_t depthPointCloudImage = NULL;
|
||||
k4a_image_t colorImageInDepth = NULL;
|
||||
k4a_image_t depthImageInColor = NULL;
|
||||
k4a_transformation_t transformation = NULL;
|
||||
|
||||
void UpdateDepthPointCloud();
|
||||
};
|
||||
@@ -15,15 +15,19 @@
|
||||
#pragma once
|
||||
|
||||
#include "utils.h"
|
||||
#include "Kinect.h"
|
||||
|
||||
struct Joint
|
||||
{
|
||||
|
||||
};
|
||||
|
||||
struct Body
|
||||
{
|
||||
Body()
|
||||
{
|
||||
bTracked = false;
|
||||
vJoints.resize(JointType_Count);
|
||||
vJointsInColorSpace.resize(JointType_Count);
|
||||
vJoints.resize(5);
|
||||
vJointsInColorSpace.resize(5);
|
||||
}
|
||||
bool bTracked;
|
||||
std::vector<Joint> vJoints;
|
||||
@@ -40,8 +44,8 @@ public:
|
||||
virtual bool AcquireFrame() = 0;
|
||||
virtual void MapDepthFrameToCameraSpace(Point3f *pCameraSpacePoints) = 0;
|
||||
virtual void MapColorFrameToCameraSpace(Point3f *pCameraSpacePoints) = 0;
|
||||
virtual void MapDepthFrameToColorSpace(Point2f *pColorSpacePoints) = 0;
|
||||
virtual void MapColorFrameToDepthSpace(Point2f *pDepthSpacePoints) = 0;
|
||||
virtual void MapDepthFrameToColorSpace(UINT16 *pColorSpacePoints) = 0;
|
||||
virtual void MapColorFrameToDepthSpace(RGB *pDepthSpacePoints) = 0;
|
||||
|
||||
bool bInitialized;
|
||||
|
||||
|
||||
@@ -68,8 +68,8 @@ private:
|
||||
/// </summary>
|
||||
void DiscardResources();
|
||||
|
||||
void DrawBody(Body &body);
|
||||
void DrawBone(Body &body, JointType joint0, JointType joint1);
|
||||
//void DrawBody(Body &body);
|
||||
//void DrawBone(Body &body, JointType joint0, JointType joint1);
|
||||
|
||||
ID2D1SolidColorBrush* m_pBrushJointTracked;
|
||||
ID2D1SolidColorBrush* m_pBrushJointInferred;
|
||||
|
||||
@@ -1,43 +0,0 @@
|
||||
// Copyright (C) 2015 Marek Kowalski (M.Kowalski@ire.pw.edu.pl), Jacek Naruniec (J.Naruniec@ire.pw.edu.pl)
|
||||
// License: MIT Software License See LICENSE.txt for the full license.
|
||||
|
||||
// If you use this software in your research, then please use the following citation:
|
||||
|
||||
// Kowalski, M.; Naruniec, J.; Daniluk, M.: "LiveScan3D: A Fast and Inexpensive 3D Data
|
||||
// Acquisition System for Multiple Kinect v2 Sensors". in 3D Vision (3DV), 2015 International Conference on, Lyon, France, 2015
|
||||
|
||||
// @INPROCEEDINGS{Kowalski15,
|
||||
// author={Kowalski, M. and Naruniec, J. and Daniluk, M.},
|
||||
// booktitle={3D Vision (3DV), 2015 International Conference on},
|
||||
// title={LiveScan3D: A Fast and Inexpensive 3D Data Acquisition System for Multiple Kinect v2 Sensors},
|
||||
// year={2015},
|
||||
// }
|
||||
#pragma once
|
||||
|
||||
#include "stdafx.h"
|
||||
#include "ICapture.h"
|
||||
#include "Kinect.h"
|
||||
#include "utils.h"
|
||||
|
||||
class KinectCapture : public ICapture
|
||||
{
|
||||
public:
|
||||
KinectCapture();
|
||||
~KinectCapture();
|
||||
|
||||
bool Initialize();
|
||||
bool AcquireFrame();
|
||||
void MapDepthFrameToCameraSpace(Point3f *pCameraSpacePoints);
|
||||
void MapColorFrameToCameraSpace(Point3f *pCameraSpacePoints);
|
||||
void MapDepthFrameToColorSpace(Point2f *pColorSpacePoints);
|
||||
void MapColorFrameToDepthSpace(Point2f *pDepthSpacePoints);
|
||||
private:
|
||||
ICoordinateMapper* pCoordinateMapper;
|
||||
IKinectSensor* pKinectSensor;
|
||||
IMultiSourceFrameReader* pMultiSourceFrameReader;
|
||||
|
||||
void GetDepthFrame(IMultiSourceFrame* pMultiFrame);
|
||||
void GetColorFrame(IMultiSourceFrame* pMultiFrame);
|
||||
void GetBodyFrame(IMultiSourceFrame* pMultiFrame);
|
||||
void GetBodyIndexFrame(IMultiSourceFrame* pMultiFrame);
|
||||
};
|
||||
@@ -19,7 +19,7 @@
|
||||
#include "SocketCS.h"
|
||||
#include "calibration.h"
|
||||
#include "utils.h"
|
||||
#include "KinectCapture.h"
|
||||
#include "azureKinectCapture.h"
|
||||
#include "frameFileWriterReader.h"
|
||||
#include <thread>
|
||||
#include <mutex>
|
||||
@@ -73,8 +73,8 @@ private:
|
||||
DWORD m_nFramesSinceUpdate;
|
||||
|
||||
Point3f* m_pCameraSpaceCoordinates;
|
||||
Point2f* m_pColorCoordinatesOfDepth;
|
||||
Point2f* m_pDepthCoordinatesOfColor;
|
||||
RGB* m_pColorInDepthSpace;
|
||||
UINT16* m_pDepthInColorSpace;
|
||||
|
||||
// Direct2D
|
||||
ImageRenderer* m_pDrawColor;
|
||||
@@ -82,8 +82,8 @@ private:
|
||||
RGB* m_pDepthRGBX;
|
||||
|
||||
void UpdateFrame();
|
||||
void ProcessColor(RGB* pBuffer, int nWidth, int nHeight);
|
||||
void ProcessDepth(const UINT16* pBuffer, int nHeight, int nWidth);
|
||||
void ShowColor();
|
||||
void ShowDepth();
|
||||
|
||||
bool SetStatusMessage(_In_z_ WCHAR* szMessage, DWORD nShowTimeMsec, bool bForce);
|
||||
|
||||
@@ -91,7 +91,7 @@ private:
|
||||
void SendFrame(vector<Point3s> vertices, vector<RGB> RGB, vector<Body> body);
|
||||
|
||||
void SocketThreadFunction();
|
||||
void StoreFrame(Point3f *vertices, Point2f *mapping, RGB *color, vector<Body> &bodies, BYTE* bodyIndex);
|
||||
void StoreFrame(Point3f *vertices, RGB *colorInDepth, vector<Body> &bodies, BYTE* bodyIndex);
|
||||
void ShowFPS();
|
||||
void ReadIPFromFile();
|
||||
void WriteIPToFile();
|
||||
|
||||
@@ -0,0 +1,253 @@
|
||||
#include "azureKinectCapture.h"
|
||||
#include <chrono>
|
||||
|
||||
AzureKinectCapture::AzureKinectCapture()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
AzureKinectCapture::~AzureKinectCapture()
|
||||
{
|
||||
k4a_image_release(colorImage);
|
||||
k4a_image_release(depthImage);
|
||||
k4a_image_release(depthPointCloudImage);
|
||||
k4a_image_release(colorImageInDepth);
|
||||
k4a_image_release(depthImageInColor);
|
||||
k4a_transformation_destroy(transformation);
|
||||
k4a_device_close(kinectSensor);
|
||||
}
|
||||
|
||||
bool AzureKinectCapture::Initialize()
|
||||
{
|
||||
return Initialize(K4A_DEVICE_DEFAULT);
|
||||
}
|
||||
|
||||
bool AzureKinectCapture::Initialize(int deviceIdx)
|
||||
{
|
||||
uint32_t count = k4a_device_get_installed_count();
|
||||
|
||||
kinectSensor = NULL;
|
||||
if (K4A_FAILED(k4a_device_open(deviceIdx, &kinectSensor)))
|
||||
{
|
||||
bInitialized = false;
|
||||
return bInitialized;
|
||||
}
|
||||
|
||||
k4a_device_configuration_t config = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
|
||||
config.camera_fps = K4A_FRAMES_PER_SECOND_30;
|
||||
config.color_format = K4A_IMAGE_FORMAT_COLOR_BGRA32;
|
||||
config.color_resolution = K4A_COLOR_RESOLUTION_1080P;
|
||||
config.depth_mode = K4A_DEPTH_MODE_NFOV_UNBINNED;
|
||||
|
||||
// Start the camera with the given configuration
|
||||
bInitialized = K4A_SUCCEEDED(k4a_device_start_cameras(kinectSensor, &config));
|
||||
|
||||
k4a_calibration_t calibration;
|
||||
if (K4A_FAILED(k4a_device_get_calibration(kinectSensor, config.depth_mode, config.color_resolution, &calibration)))
|
||||
{
|
||||
bInitialized = false;
|
||||
return bInitialized;
|
||||
}
|
||||
transformation = k4a_transformation_create(&calibration);
|
||||
|
||||
std::chrono::time_point<std::chrono::system_clock> start = std::chrono::system_clock::now();
|
||||
bool bTemp;
|
||||
do
|
||||
{
|
||||
bTemp = AcquireFrame();
|
||||
|
||||
std::chrono::duration<double> elapsedSeconds = std::chrono::system_clock::now() - start;
|
||||
if (elapsedSeconds.count() > 5.0)
|
||||
{
|
||||
bInitialized = false;
|
||||
break;
|
||||
}
|
||||
} while (!bTemp);
|
||||
|
||||
|
||||
return bInitialized;
|
||||
}
|
||||
|
||||
bool AzureKinectCapture::AcquireFrame()
|
||||
{
|
||||
if (!bInitialized)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
k4a_capture_t capture = NULL;
|
||||
|
||||
k4a_wait_result_t captureResult = k4a_device_get_capture(kinectSensor, &capture, captureTimeoutMs);
|
||||
if (captureResult != K4A_WAIT_RESULT_SUCCEEDED)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
k4a_image_release(colorImage);
|
||||
k4a_image_release(depthImage);
|
||||
|
||||
colorImage = k4a_capture_get_color_image(capture);
|
||||
depthImage = k4a_capture_get_depth_image(capture);
|
||||
if (colorImage == NULL || depthImage == NULL)
|
||||
return false;
|
||||
|
||||
|
||||
if (pColorRGBX == NULL)
|
||||
{
|
||||
nColorFrameHeight = k4a_image_get_height_pixels(colorImage);
|
||||
nColorFrameWidth = k4a_image_get_width_pixels(colorImage);
|
||||
pColorRGBX = new RGB[nColorFrameWidth * nColorFrameHeight];
|
||||
}
|
||||
|
||||
if (pDepth == NULL)
|
||||
{
|
||||
nDepthFrameHeight = k4a_image_get_height_pixels(depthImage);
|
||||
nDepthFrameWidth = k4a_image_get_width_pixels(depthImage);
|
||||
pDepth = new UINT16[nDepthFrameHeight * nDepthFrameWidth];
|
||||
}
|
||||
|
||||
memcpy(pColorRGBX, k4a_image_get_buffer(colorImage), nColorFrameWidth * nColorFrameHeight * sizeof(RGB));
|
||||
memcpy(pDepth, k4a_image_get_buffer(depthImage), nDepthFrameHeight * nDepthFrameWidth * sizeof(UINT16));
|
||||
|
||||
k4a_capture_release(capture);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void AzureKinectCapture::UpdateDepthPointCloud()
|
||||
{
|
||||
if (depthPointCloudImage == NULL)
|
||||
{
|
||||
k4a_image_create(K4A_IMAGE_FORMAT_CUSTOM16, nDepthFrameWidth, nDepthFrameHeight,
|
||||
nDepthFrameWidth * 3 * (int)sizeof(int16_t),
|
||||
&depthPointCloudImage);
|
||||
}
|
||||
|
||||
k4a_transformation_depth_image_to_point_cloud(transformation, depthImage, K4A_CALIBRATION_TYPE_DEPTH, depthPointCloudImage);
|
||||
}
|
||||
|
||||
void AzureKinectCapture::MapDepthFrameToCameraSpace(Point3f *pCameraSpacePoints)
|
||||
{
|
||||
UpdateDepthPointCloud();
|
||||
|
||||
int16_t* pointCloudData = (int16_t*)k4a_image_get_buffer(depthPointCloudImage);
|
||||
|
||||
for (int i = 0; i < nDepthFrameHeight; i++)
|
||||
{
|
||||
for (int j = 0; j < nDepthFrameWidth; j++)
|
||||
{
|
||||
pCameraSpacePoints[j + i * nDepthFrameWidth].X = pointCloudData[3 * (j + i * nDepthFrameWidth) + 0] / 1000.0f;
|
||||
pCameraSpacePoints[j + i * nDepthFrameWidth].Y = pointCloudData[3 * (j + i * nDepthFrameWidth) + 1] / 1000.0f;
|
||||
pCameraSpacePoints[j + i * nDepthFrameWidth].Z = pointCloudData[3 * (j + i * nDepthFrameWidth) + 2] / 1000.0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// This mapping is much slower then the other ones, use with caution
|
||||
void AzureKinectCapture::MapColorFrameToCameraSpace(Point3f *pCameraSpacePoints)
|
||||
{
|
||||
UpdateDepthPointCloud();
|
||||
|
||||
// Initializing temporary images
|
||||
k4a_image_t colorPointCloudImageX, colorPointCloudImageY, colorPointCloudImageZ, dummy_output_image;
|
||||
k4a_image_create(K4A_IMAGE_FORMAT_CUSTOM16, nColorFrameWidth, nColorFrameHeight,
|
||||
nColorFrameWidth * (int)sizeof(int16_t),
|
||||
&colorPointCloudImageX);
|
||||
k4a_image_create(K4A_IMAGE_FORMAT_CUSTOM16, nColorFrameWidth, nColorFrameHeight,
|
||||
nColorFrameWidth * (int)sizeof(int16_t),
|
||||
&colorPointCloudImageY);
|
||||
k4a_image_create(K4A_IMAGE_FORMAT_CUSTOM16, nColorFrameWidth, nColorFrameHeight,
|
||||
nColorFrameWidth * (int)sizeof(int16_t),
|
||||
&colorPointCloudImageZ);
|
||||
k4a_image_create(K4A_IMAGE_FORMAT_DEPTH16, nColorFrameWidth, nColorFrameHeight,
|
||||
nColorFrameWidth * (int)sizeof(int16_t),
|
||||
&dummy_output_image);
|
||||
|
||||
k4a_image_t depthPointCloudX, depthPointCloudY, depthPointCloudZ;
|
||||
|
||||
k4a_image_create(K4A_IMAGE_FORMAT_CUSTOM16, nDepthFrameWidth, nDepthFrameHeight,
|
||||
nDepthFrameWidth * (int)sizeof(int16_t),
|
||||
&depthPointCloudX);
|
||||
k4a_image_create(K4A_IMAGE_FORMAT_CUSTOM16, nDepthFrameWidth, nDepthFrameHeight,
|
||||
nDepthFrameWidth * (int)sizeof(int16_t),
|
||||
&depthPointCloudY);
|
||||
k4a_image_create(K4A_IMAGE_FORMAT_CUSTOM16, nDepthFrameWidth, nDepthFrameHeight,
|
||||
nDepthFrameWidth * (int)sizeof(int16_t),
|
||||
&depthPointCloudZ);
|
||||
|
||||
int16_t* depthPointCloudImage_buffer = (int16_t*)k4a_image_get_buffer(depthPointCloudImage);
|
||||
int16_t* depthPointCloudImageX_buffer = (int16_t*)k4a_image_get_buffer(depthPointCloudX);
|
||||
int16_t* depthPointCloudImageY_buffer = (int16_t*)k4a_image_get_buffer(depthPointCloudY);
|
||||
int16_t* depthPointCloudImageZ_buffer = (int16_t*)k4a_image_get_buffer(depthPointCloudZ);
|
||||
for (int i = 0; i < nDepthFrameWidth * nDepthFrameHeight; i++)
|
||||
{
|
||||
depthPointCloudImageX_buffer[i] = depthPointCloudImage_buffer[3 * i + 0];
|
||||
depthPointCloudImageY_buffer[i] = depthPointCloudImage_buffer[3 * i + 1];
|
||||
depthPointCloudImageZ_buffer[i] = depthPointCloudImage_buffer[3 * i + 2];
|
||||
}
|
||||
|
||||
int test = 1 << 16;
|
||||
|
||||
// Transforming per-depth-pixel point cloud to per-color-pixel point cloud
|
||||
k4a_transformation_depth_image_to_color_camera_custom(transformation, depthImage, depthPointCloudX,
|
||||
dummy_output_image, colorPointCloudImageX,
|
||||
K4A_TRANSFORMATION_INTERPOLATION_TYPE_LINEAR, 0);
|
||||
k4a_transformation_depth_image_to_color_camera_custom(transformation, depthImage, depthPointCloudY,
|
||||
dummy_output_image, colorPointCloudImageY,
|
||||
K4A_TRANSFORMATION_INTERPOLATION_TYPE_LINEAR, 0);
|
||||
k4a_transformation_depth_image_to_color_camera_custom(transformation, depthImage, depthPointCloudZ,
|
||||
dummy_output_image, colorPointCloudImageZ,
|
||||
K4A_TRANSFORMATION_INTERPOLATION_TYPE_LINEAR, 0);
|
||||
|
||||
|
||||
int16_t* pointCloudDataX = (int16_t*)k4a_image_get_buffer(colorPointCloudImageX);
|
||||
int16_t* pointCloudDataY = (int16_t*)k4a_image_get_buffer(colorPointCloudImageY);
|
||||
int16_t* pointCloudDataZ = (int16_t*)k4a_image_get_buffer(colorPointCloudImageZ);
|
||||
for (int i = 0; i < nColorFrameHeight; i++)
|
||||
{
|
||||
for (int j = 0; j < nColorFrameWidth; j++)
|
||||
{
|
||||
pCameraSpacePoints[j + i * nColorFrameWidth].X = pointCloudDataX[j + i * nColorFrameWidth + 0] / 1000.0f;
|
||||
pCameraSpacePoints[j + i * nColorFrameWidth].Y = pointCloudDataY[j + i * nColorFrameWidth + 1] / 1000.0f;
|
||||
pCameraSpacePoints[j + i * nColorFrameWidth].Z = pointCloudDataZ[j + i * nColorFrameWidth + 2] / 1000.0f;
|
||||
}
|
||||
}
|
||||
|
||||
k4a_image_release(colorPointCloudImageX);
|
||||
k4a_image_release(colorPointCloudImageY);
|
||||
k4a_image_release(colorPointCloudImageZ);
|
||||
|
||||
k4a_image_release(dummy_output_image);
|
||||
k4a_image_release(depthPointCloudX);
|
||||
k4a_image_release(depthPointCloudY);
|
||||
k4a_image_release(depthPointCloudZ);
|
||||
}
|
||||
|
||||
|
||||
void AzureKinectCapture::MapDepthFrameToColorSpace(UINT16 *pDepthInColorSpace)
|
||||
{
|
||||
if (depthImageInColor == NULL)
|
||||
{
|
||||
k4a_image_create(K4A_IMAGE_FORMAT_DEPTH16, nColorFrameWidth, nColorFrameHeight,
|
||||
nColorFrameWidth * (int)sizeof(uint16_t),
|
||||
&depthImageInColor);
|
||||
}
|
||||
|
||||
k4a_transformation_depth_image_to_color_camera(transformation, depthImage, depthImageInColor);
|
||||
|
||||
memcpy(pDepthInColorSpace, k4a_image_get_buffer(depthImageInColor), nColorFrameHeight * nColorFrameWidth * (int)sizeof(uint16_t));
|
||||
}
|
||||
|
||||
void AzureKinectCapture::MapColorFrameToDepthSpace(RGB *pColorInDepthSpace)
|
||||
{
|
||||
if (colorImageInDepth == NULL)
|
||||
{
|
||||
k4a_image_create(K4A_IMAGE_FORMAT_COLOR_BGRA32, nDepthFrameWidth, nDepthFrameHeight,
|
||||
nDepthFrameWidth * 4 * (int)sizeof(uint8_t),
|
||||
&colorImageInDepth);
|
||||
}
|
||||
|
||||
k4a_transformation_color_image_to_depth_camera(transformation, depthImage, colorImage, colorImageInDepth);
|
||||
|
||||
memcpy(pColorInDepthSpace, k4a_image_get_buffer(colorImageInDepth), nDepthFrameHeight * nDepthFrameWidth * 4 * (int)sizeof(uint8_t));
|
||||
}
|
||||
@@ -14,7 +14,6 @@
|
||||
// }
|
||||
|
||||
#include "calibration.h"
|
||||
#include "Kinect.h"
|
||||
#include "opencv\cv.h"
|
||||
|
||||
#include <fstream>
|
||||
@@ -262,7 +261,7 @@ bool Calibration::GetMarkerCorners3D(vector<Point3f> &marker3D, MarkerInfo &mark
|
||||
Point3f pointXMinYMax = pCameraCoordinates[minX + maxY * cColorWidth];
|
||||
Point3f pointMax = pCameraCoordinates[maxX + maxY * cColorWidth];
|
||||
|
||||
if (pointMin.Z < 0 || pointXMaxYMin.Z < 0 || pointXMinYMax.Z < 0 || pointMax.Z < 0)
|
||||
if (pointMin.Z <= 0 || pointXMaxYMin.Z <= 0 || pointXMinYMax.Z <= 0 || pointMax.Z <= 0)
|
||||
return false;
|
||||
|
||||
marker3D[i].X = (1 - dx) * (1 - dy) * pointMin.X + dx * (1 - dy) * pointXMaxYMin.X + (1 - dx) * dy * pointXMinYMax.X + dx * dy * pointMax.X;
|
||||
|
||||
@@ -167,7 +167,7 @@ HRESULT ImageRenderer::Draw(BYTE* pImage, unsigned long cbImage, std::vector<Bod
|
||||
{
|
||||
if (vBodies[i].bTracked)
|
||||
{
|
||||
DrawBody(vBodies[i]);
|
||||
//DrawBody(vBodies[i]);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -188,62 +188,62 @@ HRESULT ImageRenderer::Draw(BYTE* pImage, unsigned long cbImage, std::vector<Bod
|
||||
/// Draws all the body to the associated hwnd, assumes that BeginDraw has been called
|
||||
/// </summary>
|
||||
/// <param name="body">body to be drawn</param>
|
||||
void ImageRenderer::DrawBody(Body &body)
|
||||
{
|
||||
// Draw the bones
|
||||
|
||||
// Torso
|
||||
DrawBone(body, JointType_Head, JointType_Neck);
|
||||
DrawBone(body, JointType_Neck, JointType_SpineShoulder);
|
||||
DrawBone(body, JointType_SpineShoulder, JointType_SpineMid);
|
||||
DrawBone(body, JointType_SpineMid, JointType_SpineBase);
|
||||
DrawBone(body, JointType_SpineShoulder, JointType_ShoulderRight);
|
||||
DrawBone(body, JointType_SpineShoulder, JointType_ShoulderLeft);
|
||||
DrawBone(body, JointType_SpineBase, JointType_HipRight);
|
||||
DrawBone(body, JointType_SpineBase, JointType_HipLeft);
|
||||
|
||||
// Right Arm
|
||||
DrawBone(body, JointType_ShoulderRight, JointType_ElbowRight);
|
||||
DrawBone(body, JointType_ElbowRight, JointType_WristRight);
|
||||
DrawBone(body, JointType_WristRight, JointType_HandRight);
|
||||
DrawBone(body, JointType_HandRight, JointType_HandTipRight);
|
||||
DrawBone(body, JointType_WristRight, JointType_ThumbRight);
|
||||
|
||||
// Left Arm
|
||||
DrawBone(body, JointType_ShoulderLeft, JointType_ElbowLeft);
|
||||
DrawBone(body, JointType_ElbowLeft, JointType_WristLeft);
|
||||
DrawBone(body, JointType_WristLeft, JointType_HandLeft);
|
||||
DrawBone(body, JointType_HandLeft, JointType_HandTipLeft);
|
||||
DrawBone(body, JointType_WristLeft, JointType_ThumbLeft);
|
||||
|
||||
// Right Leg
|
||||
DrawBone(body, JointType_HipRight, JointType_KneeRight);
|
||||
DrawBone(body, JointType_KneeRight, JointType_AnkleRight);
|
||||
DrawBone(body, JointType_AnkleRight, JointType_FootRight);
|
||||
|
||||
// Left Leg
|
||||
DrawBone(body, JointType_HipLeft, JointType_KneeLeft);
|
||||
DrawBone(body, JointType_KneeLeft, JointType_AnkleLeft);
|
||||
DrawBone(body, JointType_AnkleLeft, JointType_FootLeft);
|
||||
|
||||
for (unsigned int i = 0; i < body.vJoints.size(); i++)
|
||||
{
|
||||
D2D1_POINT_2F tempPoint;
|
||||
tempPoint.x = body.vJointsInColorSpace[i].X;
|
||||
tempPoint.y = body.vJointsInColorSpace[i].Y;
|
||||
|
||||
D2D1_ELLIPSE ellipse = D2D1::Ellipse(tempPoint, 6.0f, 6.0f);
|
||||
|
||||
if (body.vJoints[i].TrackingState == TrackingState_Inferred)
|
||||
{
|
||||
m_pRenderTarget->FillEllipse(ellipse, m_pBrushJointInferred);
|
||||
}
|
||||
else if (body.vJoints[i].TrackingState == TrackingState_Tracked)
|
||||
{
|
||||
m_pRenderTarget->FillEllipse(ellipse, m_pBrushJointTracked);
|
||||
}
|
||||
}
|
||||
}
|
||||
//void ImageRenderer::DrawBody(Body &body)
|
||||
//{
|
||||
// // Draw the bones
|
||||
//
|
||||
// // Torso
|
||||
// DrawBone(body, JointType_Head, JointType_Neck);
|
||||
// DrawBone(body, JointType_Neck, JointType_SpineShoulder);
|
||||
// DrawBone(body, JointType_SpineShoulder, JointType_SpineMid);
|
||||
// DrawBone(body, JointType_SpineMid, JointType_SpineBase);
|
||||
// DrawBone(body, JointType_SpineShoulder, JointType_ShoulderRight);
|
||||
// DrawBone(body, JointType_SpineShoulder, JointType_ShoulderLeft);
|
||||
// DrawBone(body, JointType_SpineBase, JointType_HipRight);
|
||||
// DrawBone(body, JointType_SpineBase, JointType_HipLeft);
|
||||
//
|
||||
// // Right Arm
|
||||
// DrawBone(body, JointType_ShoulderRight, JointType_ElbowRight);
|
||||
// DrawBone(body, JointType_ElbowRight, JointType_WristRight);
|
||||
// DrawBone(body, JointType_WristRight, JointType_HandRight);
|
||||
// DrawBone(body, JointType_HandRight, JointType_HandTipRight);
|
||||
// DrawBone(body, JointType_WristRight, JointType_ThumbRight);
|
||||
//
|
||||
// // Left Arm
|
||||
// DrawBone(body, JointType_ShoulderLeft, JointType_ElbowLeft);
|
||||
// DrawBone(body, JointType_ElbowLeft, JointType_WristLeft);
|
||||
// DrawBone(body, JointType_WristLeft, JointType_HandLeft);
|
||||
// DrawBone(body, JointType_HandLeft, JointType_HandTipLeft);
|
||||
// DrawBone(body, JointType_WristLeft, JointType_ThumbLeft);
|
||||
//
|
||||
// // Right Leg
|
||||
// DrawBone(body, JointType_HipRight, JointType_KneeRight);
|
||||
// DrawBone(body, JointType_KneeRight, JointType_AnkleRight);
|
||||
// DrawBone(body, JointType_AnkleRight, JointType_FootRight);
|
||||
//
|
||||
// // Left Leg
|
||||
// DrawBone(body, JointType_HipLeft, JointType_KneeLeft);
|
||||
// DrawBone(body, JointType_KneeLeft, JointType_AnkleLeft);
|
||||
// DrawBone(body, JointType_AnkleLeft, JointType_FootLeft);
|
||||
//
|
||||
// for (unsigned int i = 0; i < body.vJoints.size(); i++)
|
||||
// {
|
||||
// D2D1_POINT_2F tempPoint;
|
||||
// tempPoint.x = body.vJointsInColorSpace[i].X;
|
||||
// tempPoint.y = body.vJointsInColorSpace[i].Y;
|
||||
//
|
||||
// D2D1_ELLIPSE ellipse = D2D1::Ellipse(tempPoint, 6.0f, 6.0f);
|
||||
//
|
||||
// if (body.vJoints[i].TrackingState == TrackingState_Inferred)
|
||||
// {
|
||||
// m_pRenderTarget->FillEllipse(ellipse, m_pBrushJointInferred);
|
||||
// }
|
||||
// else if (body.vJoints[i].TrackingState == TrackingState_Tracked)
|
||||
// {
|
||||
// m_pRenderTarget->FillEllipse(ellipse, m_pBrushJointTracked);
|
||||
// }
|
||||
// }
|
||||
//}
|
||||
|
||||
/// <summary>
|
||||
/// Draws one bone of a body (joint to joint)
|
||||
@@ -251,35 +251,35 @@ void ImageRenderer::DrawBody(Body &body)
|
||||
/// <param name="body">body to be drawn</param>
|
||||
/// <param name="joint0">one joint of the bone to draw</param>
|
||||
/// <param name="joint1">other joint of the bone to draw</param>
|
||||
void ImageRenderer::DrawBone(Body &body, JointType joint0, JointType joint1)
|
||||
{
|
||||
TrackingState joint0State = body.vJoints[joint0].TrackingState;
|
||||
TrackingState joint1State = body.vJoints[joint1].TrackingState;
|
||||
|
||||
// If we can't find either of these joints, exit
|
||||
if ((joint0State == TrackingState_NotTracked) || (joint1State == TrackingState_NotTracked))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
// Don't draw if both points are inferred
|
||||
if ((joint0State == TrackingState_Inferred) && (joint1State == TrackingState_Inferred))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
D2D1_POINT_2F joint0Point, joint1Point;
|
||||
joint0Point.x = body.vJointsInColorSpace[joint0].X;
|
||||
joint0Point.y = body.vJointsInColorSpace[joint0].Y;
|
||||
joint1Point.x = body.vJointsInColorSpace[joint1].X;
|
||||
joint1Point.y = body.vJointsInColorSpace[joint1].Y;
|
||||
// We assume all drawn bones are inferred unless BOTH joints are tracked
|
||||
if ((joint0State == TrackingState_Tracked) && (joint1State == TrackingState_Tracked))
|
||||
{
|
||||
m_pRenderTarget->DrawLine(joint0Point, joint1Point, m_pBrushBoneTracked, 10.0f);
|
||||
}
|
||||
else
|
||||
{
|
||||
m_pRenderTarget->DrawLine(joint0Point, joint1Point, m_pBrushBoneInferred, 3);
|
||||
}
|
||||
}
|
||||
//void ImageRenderer::DrawBone(Body &body, JointType joint0, JointType joint1)
|
||||
//{
|
||||
// TrackingState joint0State = body.vJoints[joint0].TrackingState;
|
||||
// TrackingState joint1State = body.vJoints[joint1].TrackingState;
|
||||
//
|
||||
// // If we can't find either of these joints, exit
|
||||
// if ((joint0State == TrackingState_NotTracked) || (joint1State == TrackingState_NotTracked))
|
||||
// {
|
||||
// return;
|
||||
// }
|
||||
//
|
||||
// // Don't draw if both points are inferred
|
||||
// if ((joint0State == TrackingState_Inferred) && (joint1State == TrackingState_Inferred))
|
||||
// {
|
||||
// return;
|
||||
// }
|
||||
//
|
||||
// D2D1_POINT_2F joint0Point, joint1Point;
|
||||
// joint0Point.x = body.vJointsInColorSpace[joint0].X;
|
||||
// joint0Point.y = body.vJointsInColorSpace[joint0].Y;
|
||||
// joint1Point.x = body.vJointsInColorSpace[joint1].X;
|
||||
// joint1Point.y = body.vJointsInColorSpace[joint1].Y;
|
||||
// // We assume all drawn bones are inferred unless BOTH joints are tracked
|
||||
// if ((joint0State == TrackingState_Tracked) && (joint1State == TrackingState_Tracked))
|
||||
// {
|
||||
// m_pRenderTarget->DrawLine(joint0Point, joint1Point, m_pBrushBoneTracked, 10.0f);
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// m_pRenderTarget->DrawLine(joint0Point, joint1Point, m_pBrushBoneInferred, 3);
|
||||
// }
|
||||
//}
|
||||
@@ -1,250 +0,0 @@
|
||||
// Copyright (C) 2015 Marek Kowalski (M.Kowalski@ire.pw.edu.pl), Jacek Naruniec (J.Naruniec@ire.pw.edu.pl)
|
||||
// License: MIT Software License See LICENSE.txt for the full license.
|
||||
|
||||
// If you use this software in your research, then please use the following citation:
|
||||
|
||||
// Kowalski, M.; Naruniec, J.; Daniluk, M.: "LiveScan3D: A Fast and Inexpensive 3D Data
|
||||
// Acquisition System for Multiple Kinect v2 Sensors". in 3D Vision (3DV), 2015 International Conference on, Lyon, France, 2015
|
||||
|
||||
// @INPROCEEDINGS{Kowalski15,
|
||||
// author={Kowalski, M. and Naruniec, J. and Daniluk, M.},
|
||||
// booktitle={3D Vision (3DV), 2015 International Conference on},
|
||||
// title={LiveScan3D: A Fast and Inexpensive 3D Data Acquisition System for Multiple Kinect v2 Sensors},
|
||||
// year={2015},
|
||||
// }
|
||||
#include "KinectCapture.h"
|
||||
#include <chrono>
|
||||
|
||||
KinectCapture::KinectCapture()
|
||||
{
|
||||
pKinectSensor = NULL;
|
||||
pCoordinateMapper = NULL;
|
||||
pMultiSourceFrameReader = NULL;
|
||||
}
|
||||
|
||||
KinectCapture::~KinectCapture()
|
||||
{
|
||||
SafeRelease(pKinectSensor);
|
||||
SafeRelease(pCoordinateMapper);
|
||||
SafeRelease(pMultiSourceFrameReader);
|
||||
}
|
||||
|
||||
bool KinectCapture::Initialize()
|
||||
{
|
||||
HRESULT hr;
|
||||
|
||||
hr = GetDefaultKinectSensor(&pKinectSensor);
|
||||
if (FAILED(hr))
|
||||
{
|
||||
bInitialized = false;
|
||||
return bInitialized;
|
||||
}
|
||||
|
||||
if (pKinectSensor)
|
||||
{
|
||||
pKinectSensor->get_CoordinateMapper(&pCoordinateMapper);
|
||||
hr = pKinectSensor->Open();
|
||||
|
||||
if (SUCCEEDED(hr))
|
||||
{
|
||||
pKinectSensor->OpenMultiSourceFrameReader(FrameSourceTypes::FrameSourceTypes_Color |
|
||||
FrameSourceTypes::FrameSourceTypes_Depth |
|
||||
FrameSourceTypes::FrameSourceTypes_Body |
|
||||
FrameSourceTypes::FrameSourceTypes_BodyIndex,
|
||||
&pMultiSourceFrameReader);
|
||||
}
|
||||
}
|
||||
|
||||
bInitialized = SUCCEEDED(hr);
|
||||
|
||||
if (bInitialized)
|
||||
{
|
||||
std::chrono::time_point<std::chrono::system_clock> start = std::chrono::system_clock::now();
|
||||
bool bTemp;
|
||||
do
|
||||
{
|
||||
bTemp = AcquireFrame();
|
||||
|
||||
std::chrono::duration<double> elapsedSeconds = std::chrono::system_clock::now() - start;
|
||||
if (elapsedSeconds.count() > 5.0)
|
||||
{
|
||||
bInitialized = false;
|
||||
break;
|
||||
}
|
||||
|
||||
} while (!bTemp);
|
||||
}
|
||||
|
||||
return bInitialized;
|
||||
}
|
||||
|
||||
bool KinectCapture::AcquireFrame()
|
||||
{
|
||||
if (!bInitialized)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
//Multi frame
|
||||
IMultiSourceFrame* pMultiFrame = NULL;
|
||||
HRESULT hr = pMultiSourceFrameReader->AcquireLatestFrame(&pMultiFrame);
|
||||
|
||||
if (!SUCCEEDED(hr))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
GetDepthFrame(pMultiFrame);
|
||||
GetColorFrame(pMultiFrame);
|
||||
GetBodyFrame(pMultiFrame);
|
||||
GetBodyIndexFrame(pMultiFrame);
|
||||
|
||||
SafeRelease(pMultiFrame);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void KinectCapture::MapDepthFrameToCameraSpace(Point3f *pCameraSpacePoints)
|
||||
{
|
||||
pCoordinateMapper->MapDepthFrameToCameraSpace(nDepthFrameWidth * nDepthFrameHeight, pDepth, nDepthFrameWidth * nDepthFrameHeight, (CameraSpacePoint*)pCameraSpacePoints);
|
||||
}
|
||||
|
||||
void KinectCapture::MapColorFrameToCameraSpace(Point3f *pCameraSpacePoints)
|
||||
{
|
||||
pCoordinateMapper->MapColorFrameToCameraSpace(nDepthFrameWidth * nDepthFrameHeight, pDepth, nColorFrameWidth * nColorFrameHeight, (CameraSpacePoint*)pCameraSpacePoints);
|
||||
}
|
||||
|
||||
void KinectCapture::MapDepthFrameToColorSpace(Point2f *pColorSpacePoints)
|
||||
{
|
||||
pCoordinateMapper->MapDepthFrameToColorSpace(nDepthFrameWidth * nDepthFrameHeight, pDepth, nDepthFrameWidth * nDepthFrameHeight, (ColorSpacePoint*)pColorSpacePoints);
|
||||
}
|
||||
|
||||
void KinectCapture::MapColorFrameToDepthSpace(Point2f *pDepthSpacePoints)
|
||||
{
|
||||
pCoordinateMapper->MapColorFrameToDepthSpace(nDepthFrameWidth * nDepthFrameHeight, pDepth, nColorFrameWidth * nColorFrameHeight, (DepthSpacePoint*)pDepthSpacePoints);;
|
||||
}
|
||||
|
||||
void KinectCapture::GetDepthFrame(IMultiSourceFrame* pMultiFrame)
|
||||
{
|
||||
IDepthFrameReference* pDepthFrameReference = NULL;
|
||||
IDepthFrame* pDepthFrame = NULL;
|
||||
pMultiFrame->get_DepthFrameReference(&pDepthFrameReference);
|
||||
HRESULT hr = pDepthFrameReference->AcquireFrame(&pDepthFrame);
|
||||
|
||||
if (SUCCEEDED(hr))
|
||||
{
|
||||
if (pDepth == NULL)
|
||||
{
|
||||
IFrameDescription* pFrameDescription = NULL;
|
||||
hr = pDepthFrame->get_FrameDescription(&pFrameDescription);
|
||||
pFrameDescription->get_Width(&nDepthFrameWidth);
|
||||
pFrameDescription->get_Height(&nDepthFrameHeight);
|
||||
pDepth = new UINT16[nDepthFrameHeight * nDepthFrameWidth];
|
||||
SafeRelease(pFrameDescription);
|
||||
}
|
||||
|
||||
UINT nBufferSize = nDepthFrameHeight * nDepthFrameWidth;
|
||||
hr = pDepthFrame->CopyFrameDataToArray(nBufferSize, pDepth);
|
||||
}
|
||||
|
||||
SafeRelease(pDepthFrame);
|
||||
SafeRelease(pDepthFrameReference);
|
||||
}
|
||||
|
||||
void KinectCapture::GetColorFrame(IMultiSourceFrame* pMultiFrame)
|
||||
{
|
||||
IColorFrameReference* pColorFrameReference = NULL;
|
||||
IColorFrame* pColorFrame = NULL;
|
||||
pMultiFrame->get_ColorFrameReference(&pColorFrameReference);
|
||||
HRESULT hr = pColorFrameReference->AcquireFrame(&pColorFrame);
|
||||
|
||||
if (SUCCEEDED(hr))
|
||||
{
|
||||
if (pColorRGBX == NULL)
|
||||
{
|
||||
IFrameDescription* pFrameDescription = NULL;
|
||||
hr = pColorFrame->get_FrameDescription(&pFrameDescription);
|
||||
hr = pFrameDescription->get_Width(&nColorFrameWidth);
|
||||
hr = pFrameDescription->get_Height(&nColorFrameHeight);
|
||||
pColorRGBX = new RGB[nColorFrameWidth * nColorFrameHeight];
|
||||
SafeRelease(pFrameDescription);
|
||||
}
|
||||
|
||||
UINT nBufferSize = nColorFrameWidth * nColorFrameHeight * sizeof(RGB);
|
||||
hr = pColorFrame->CopyConvertedFrameDataToArray(nBufferSize, reinterpret_cast<BYTE*>(pColorRGBX), ColorImageFormat_Bgra);
|
||||
}
|
||||
|
||||
SafeRelease(pColorFrame);
|
||||
SafeRelease(pColorFrameReference);
|
||||
}
|
||||
|
||||
void KinectCapture::GetBodyFrame(IMultiSourceFrame* pMultiFrame)
|
||||
{
|
||||
IBodyFrameReference* pBodyFrameReference = NULL;
|
||||
IBodyFrame* pBodyFrame = NULL;
|
||||
pMultiFrame->get_BodyFrameReference(&pBodyFrameReference);
|
||||
HRESULT hr = pBodyFrameReference->AcquireFrame(&pBodyFrame);
|
||||
|
||||
|
||||
if (SUCCEEDED(hr))
|
||||
{
|
||||
IBody* bodies[BODY_COUNT] = { NULL };
|
||||
pBodyFrame->GetAndRefreshBodyData(BODY_COUNT, bodies);
|
||||
|
||||
vBodies = std::vector<Body>(BODY_COUNT);
|
||||
for (int i = 0; i < BODY_COUNT; i++)
|
||||
{
|
||||
if (bodies[i])
|
||||
{
|
||||
Joint joints[JointType_Count];
|
||||
BOOLEAN isTracked;
|
||||
|
||||
bodies[i]->get_IsTracked(&isTracked);
|
||||
bodies[i]->GetJoints(JointType_Count, joints);
|
||||
|
||||
vBodies[i].vJoints.assign(joints, joints + JointType_Count);
|
||||
|
||||
if (isTracked == TRUE)
|
||||
vBodies[i].bTracked = true;
|
||||
else
|
||||
vBodies[i].bTracked = false;
|
||||
|
||||
vBodies[i].vJointsInColorSpace.resize(JointType_Count);
|
||||
|
||||
for (int j = 0; j < JointType_Count; j++)
|
||||
{
|
||||
ColorSpacePoint tempPoint;
|
||||
pCoordinateMapper->MapCameraPointToColorSpace(joints[j].Position, &tempPoint);
|
||||
vBodies[i].vJointsInColorSpace[j].X = tempPoint.X;
|
||||
vBodies[i].vJointsInColorSpace[j].Y = tempPoint.Y;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
SafeRelease(pBodyFrame);
|
||||
SafeRelease(pBodyFrameReference);
|
||||
}
|
||||
|
||||
void KinectCapture::GetBodyIndexFrame(IMultiSourceFrame* pMultiFrame)
|
||||
{
|
||||
IBodyIndexFrameReference* pBodyIndexFrameReference = NULL;
|
||||
IBodyIndexFrame* pBodyIndexFrame = NULL;
|
||||
pMultiFrame->get_BodyIndexFrameReference(&pBodyIndexFrameReference);
|
||||
HRESULT hr = pBodyIndexFrameReference->AcquireFrame(&pBodyIndexFrame);
|
||||
|
||||
|
||||
if (SUCCEEDED(hr))
|
||||
{
|
||||
if (pBodyIndex == NULL)
|
||||
{
|
||||
pBodyIndex = new BYTE[nDepthFrameHeight * nDepthFrameWidth];
|
||||
}
|
||||
|
||||
UINT nBufferSize = nDepthFrameHeight * nDepthFrameWidth;
|
||||
hr = pBodyIndexFrame->CopyFrameDataToArray(nBufferSize, pBodyIndex);
|
||||
}
|
||||
|
||||
SafeRelease(pBodyIndexFrame);
|
||||
SafeRelease(pBodyIndexFrameReference);
|
||||
}
|
||||
@@ -47,8 +47,8 @@ LiveScanClient::LiveScanClient() :
|
||||
m_pDrawColor(NULL),
|
||||
m_pDepthRGBX(NULL),
|
||||
m_pCameraSpaceCoordinates(NULL),
|
||||
m_pColorCoordinatesOfDepth(NULL),
|
||||
m_pDepthCoordinatesOfColor(NULL),
|
||||
m_pColorInDepthSpace(NULL),
|
||||
m_pDepthInColorSpace(NULL),
|
||||
m_bCalibrate(false),
|
||||
m_bFilter(false),
|
||||
m_bStreamOnlyBodies(false),
|
||||
@@ -64,7 +64,7 @@ LiveScanClient::LiveScanClient() :
|
||||
m_nFilterNeighbors(10),
|
||||
m_fFilterThreshold(0.01f)
|
||||
{
|
||||
pCapture = new KinectCapture();
|
||||
pCapture = new AzureKinectCapture();
|
||||
|
||||
LARGE_INTEGER qpf = {0};
|
||||
if (QueryPerformanceFrequency(&qpf))
|
||||
@@ -109,16 +109,16 @@ LiveScanClient::~LiveScanClient()
|
||||
m_pCameraSpaceCoordinates = NULL;
|
||||
}
|
||||
|
||||
if (m_pColorCoordinatesOfDepth)
|
||||
if (m_pColorInDepthSpace)
|
||||
{
|
||||
delete[] m_pColorCoordinatesOfDepth;
|
||||
m_pColorCoordinatesOfDepth = NULL;
|
||||
delete[] m_pColorInDepthSpace;
|
||||
m_pColorInDepthSpace = NULL;
|
||||
}
|
||||
|
||||
if (m_pDepthCoordinatesOfColor)
|
||||
if (m_pDepthInColorSpace)
|
||||
{
|
||||
delete[] m_pDepthCoordinatesOfColor;
|
||||
m_pDepthCoordinatesOfColor = NULL;
|
||||
delete[] m_pDepthInColorSpace;
|
||||
m_pDepthInColorSpace = NULL;
|
||||
}
|
||||
|
||||
if (m_pClientSocket)
|
||||
@@ -185,8 +185,6 @@ int LiveScanClient::Run(HINSTANCE hInstance, int nCmdShow)
|
||||
return static_cast<int>(msg.wParam);
|
||||
}
|
||||
|
||||
|
||||
|
||||
void LiveScanClient::UpdateFrame()
|
||||
{
|
||||
if (!pCapture->bInitialized)
|
||||
@@ -200,10 +198,10 @@ void LiveScanClient::UpdateFrame()
|
||||
return;
|
||||
|
||||
pCapture->MapDepthFrameToCameraSpace(m_pCameraSpaceCoordinates);
|
||||
pCapture->MapDepthFrameToColorSpace(m_pColorCoordinatesOfDepth);
|
||||
pCapture->MapColorFrameToDepthSpace(m_pColorInDepthSpace);
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(m_mSocketThreadMutex);
|
||||
StoreFrame(m_pCameraSpaceCoordinates, m_pColorCoordinatesOfDepth, pCapture->pColorRGBX, pCapture->vBodies, pCapture->pBodyIndex);
|
||||
StoreFrame(m_pCameraSpaceCoordinates, m_pColorInDepthSpace, pCapture->vBodies, pCapture->pBodyIndex);
|
||||
|
||||
if (m_bCaptureFrame)
|
||||
{
|
||||
@@ -231,9 +229,9 @@ void LiveScanClient::UpdateFrame()
|
||||
}
|
||||
|
||||
if (!m_bShowDepth)
|
||||
ProcessColor(pCapture->pColorRGBX, pCapture->nColorFrameWidth, pCapture->nColorFrameHeight);
|
||||
ShowColor();
|
||||
else
|
||||
ProcessDepth(pCapture->pDepth, pCapture->nDepthFrameWidth, pCapture->nDepthFrameHeight);
|
||||
ShowDepth();
|
||||
|
||||
ShowFPS();
|
||||
}
|
||||
@@ -280,10 +278,10 @@ LRESULT CALLBACK LiveScanClient::DlgProc(HWND hWnd, UINT message, WPARAM wParam,
|
||||
if (res)
|
||||
{
|
||||
m_pDepthRGBX = new RGB[pCapture->nColorFrameWidth * pCapture->nColorFrameHeight];
|
||||
m_pDepthInColorSpace = new UINT16[pCapture->nColorFrameWidth * pCapture->nColorFrameHeight];
|
||||
|
||||
m_pCameraSpaceCoordinates = new Point3f[pCapture->nDepthFrameWidth * pCapture->nDepthFrameHeight];
|
||||
m_pColorCoordinatesOfDepth = new Point2f[pCapture->nDepthFrameWidth * pCapture->nDepthFrameHeight];
|
||||
m_pDepthCoordinatesOfColor = new Point2f[pCapture->nColorFrameWidth * pCapture->nColorFrameHeight];
|
||||
m_pColorInDepthSpace = new RGB[pCapture->nDepthFrameWidth * pCapture->nDepthFrameHeight];
|
||||
}
|
||||
else
|
||||
{
|
||||
@@ -368,27 +366,17 @@ LRESULT CALLBACK LiveScanClient::DlgProc(HWND hWnd, UINT message, WPARAM wParam,
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
void LiveScanClient::ProcessDepth(const UINT16* pBuffer, int nWidth, int nHeight)
|
||||
void LiveScanClient::ShowDepth()
|
||||
{
|
||||
// Make sure we've received valid data
|
||||
if (m_pDepthRGBX && m_pDepthCoordinatesOfColor && pBuffer && (nWidth == pCapture->nDepthFrameWidth) && (nHeight == pCapture->nDepthFrameHeight))
|
||||
if (m_pDepthRGBX && m_pDepthInColorSpace)
|
||||
{
|
||||
// end pixel is start + width*height - 1
|
||||
const UINT16* pBufferEnd = pBuffer + (nWidth * nHeight);
|
||||
|
||||
pCapture->MapColorFrameToDepthSpace(m_pDepthCoordinatesOfColor);
|
||||
pCapture->MapDepthFrameToColorSpace(m_pDepthInColorSpace);
|
||||
|
||||
for (int i = 0; i < pCapture->nColorFrameWidth * pCapture->nColorFrameHeight; i++)
|
||||
{
|
||||
Point2f depthPoint = m_pDepthCoordinatesOfColor[i];
|
||||
BYTE intensity = 0;
|
||||
|
||||
if (depthPoint.X >= 0 && depthPoint.Y >= 0)
|
||||
{
|
||||
int depthIdx = (int)(depthPoint.X + depthPoint.Y * pCapture->nDepthFrameWidth);
|
||||
USHORT depth = pBuffer[depthIdx];
|
||||
intensity = static_cast<BYTE>(depth % 256);
|
||||
}
|
||||
USHORT depth = m_pDepthInColorSpace[i];
|
||||
BYTE intensity = static_cast<BYTE>(depth % 256);
|
||||
|
||||
m_pDepthRGBX[i].rgbRed = intensity;
|
||||
m_pDepthRGBX[i].rgbGreen = intensity;
|
||||
@@ -400,13 +388,13 @@ void LiveScanClient::ProcessDepth(const UINT16* pBuffer, int nWidth, int nHeight
|
||||
}
|
||||
}
|
||||
|
||||
void LiveScanClient::ProcessColor(RGB* pBuffer, int nWidth, int nHeight)
|
||||
void LiveScanClient::ShowColor()
|
||||
{
|
||||
// Make sure we've received valid data
|
||||
if (pBuffer && (nWidth == pCapture->nColorFrameWidth) && (nHeight == pCapture->nColorFrameHeight))
|
||||
if (pCapture->pColorRGBX)
|
||||
{
|
||||
// Draw the data with Direct2D
|
||||
m_pDrawColor->Draw(reinterpret_cast<BYTE*>(pBuffer), pCapture->nColorFrameWidth * pCapture->nColorFrameHeight * sizeof(RGB), pCapture->vBodies);
|
||||
m_pDrawColor->Draw(reinterpret_cast<BYTE*>(pCapture->pColorRGBX), pCapture->nColorFrameWidth * pCapture->nColorFrameHeight * sizeof(RGB), pCapture->vBodies);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -648,24 +636,24 @@ void LiveScanClient::SendFrame(vector<Point3s> vertices, vector<RGB> RGB, vector
|
||||
|
||||
for (int j = 0; j < nJoints; j++)
|
||||
{
|
||||
//Joint
|
||||
memcpy(buffer.data() + pos, &body[i].vJoints[j].JointType, sizeof(JointType));
|
||||
pos += sizeof(JointType);
|
||||
memcpy(buffer.data() + pos, &body[i].vJoints[j].TrackingState, sizeof(TrackingState));
|
||||
pos += sizeof(TrackingState);
|
||||
//Joint position
|
||||
memcpy(buffer.data() + pos, &body[i].vJoints[j].Position.X, sizeof(float));
|
||||
pos += sizeof(float);
|
||||
memcpy(buffer.data() + pos, &body[i].vJoints[j].Position.Y, sizeof(float));
|
||||
pos += sizeof(float);
|
||||
memcpy(buffer.data() + pos, &body[i].vJoints[j].Position.Z, sizeof(float));
|
||||
pos += sizeof(float);
|
||||
////Joint
|
||||
//memcpy(buffer.data() + pos, &body[i].vJoints[j].JointType, sizeof(JointType));
|
||||
//pos += sizeof(JointType);
|
||||
//memcpy(buffer.data() + pos, &body[i].vJoints[j].TrackingState, sizeof(TrackingState));
|
||||
//pos += sizeof(TrackingState);
|
||||
////Joint position
|
||||
//memcpy(buffer.data() + pos, &body[i].vJoints[j].Position.X, sizeof(float));
|
||||
//pos += sizeof(float);
|
||||
//memcpy(buffer.data() + pos, &body[i].vJoints[j].Position.Y, sizeof(float));
|
||||
//pos += sizeof(float);
|
||||
//memcpy(buffer.data() + pos, &body[i].vJoints[j].Position.Z, sizeof(float));
|
||||
//pos += sizeof(float);
|
||||
|
||||
//JointInColorSpace
|
||||
memcpy(buffer.data() + pos, &body[i].vJointsInColorSpace[j].X, sizeof(float));
|
||||
pos += sizeof(float);
|
||||
memcpy(buffer.data() + pos, &body[i].vJointsInColorSpace[j].Y, sizeof(float));
|
||||
pos += sizeof(float);
|
||||
////JointInColorSpace
|
||||
//memcpy(buffer.data() + pos, &body[i].vJointsInColorSpace[j].X, sizeof(float));
|
||||
//pos += sizeof(float);
|
||||
//memcpy(buffer.data() + pos, &body[i].vJointsInColorSpace[j].Y, sizeof(float));
|
||||
//pos += sizeof(float);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -689,7 +677,7 @@ void LiveScanClient::SendFrame(vector<Point3s> vertices, vector<RGB> RGB, vector
|
||||
m_pClientSocket->SendBytes(buffer.data(), size);
|
||||
}
|
||||
|
||||
void LiveScanClient::StoreFrame(Point3f *vertices, Point2f *mapping, RGB *color, vector<Body> &bodies, BYTE* bodyIndex)
|
||||
void LiveScanClient::StoreFrame(Point3f *vertices, RGB *colorInDepth, vector<Body> &bodies, BYTE* bodyIndex)
|
||||
{
|
||||
std::vector<Point3f> goodVertices;
|
||||
std::vector<RGB> goodColorPoints;
|
||||
@@ -701,10 +689,10 @@ void LiveScanClient::StoreFrame(Point3f *vertices, Point2f *mapping, RGB *color,
|
||||
if (m_bStreamOnlyBodies && bodyIndex[vertexIndex] >= bodies.size())
|
||||
continue;
|
||||
|
||||
if (vertices[vertexIndex].Z >= 0 && mapping[vertexIndex].Y >= 0 && mapping[vertexIndex].Y < pCapture->nColorFrameHeight)
|
||||
if (vertices[vertexIndex].Z >= 0 && colorInDepth[vertexIndex].rgbReserved == 255)
|
||||
{
|
||||
Point3f temp = vertices[vertexIndex];
|
||||
RGB tempColor = color[(int)mapping[vertexIndex].X + (int)mapping[vertexIndex].Y * pCapture->nColorFrameWidth];
|
||||
RGB tempColor = colorInDepth[vertexIndex];
|
||||
if (calibration.bCalibrated)
|
||||
{
|
||||
temp.X += calibration.worldT[0];
|
||||
@@ -725,26 +713,26 @@ void LiveScanClient::StoreFrame(Point3f *vertices, Point2f *mapping, RGB *color,
|
||||
|
||||
vector<Body> tempBodies = bodies;
|
||||
|
||||
for (unsigned int i = 0; i < tempBodies.size(); i++)
|
||||
{
|
||||
for (unsigned int j = 0; j < tempBodies[i].vJoints.size(); j++)
|
||||
{
|
||||
if (calibration.bCalibrated)
|
||||
{
|
||||
tempBodies[i].vJoints[j].Position.X += calibration.worldT[0];
|
||||
tempBodies[i].vJoints[j].Position.Y += calibration.worldT[1];
|
||||
tempBodies[i].vJoints[j].Position.Z += calibration.worldT[2];
|
||||
//for (unsigned int i = 0; i < tempBodies.size(); i++)
|
||||
//{
|
||||
// for (unsigned int j = 0; j < tempBodies[i].vJoints.size(); j++)
|
||||
// {
|
||||
// if (calibration.bCalibrated)
|
||||
// {
|
||||
// tempBodies[i].vJoints[j].Position.X += calibration.worldT[0];
|
||||
// tempBodies[i].vJoints[j].Position.Y += calibration.worldT[1];
|
||||
// tempBodies[i].vJoints[j].Position.Z += calibration.worldT[2];
|
||||
|
||||
Point3f tempPoint(tempBodies[i].vJoints[j].Position.X, tempBodies[i].vJoints[j].Position.Y, tempBodies[i].vJoints[j].Position.Z);
|
||||
// Point3f tempPoint(tempBodies[i].vJoints[j].Position.X, tempBodies[i].vJoints[j].Position.Y, tempBodies[i].vJoints[j].Position.Z);
|
||||
|
||||
tempPoint = RotatePoint(tempPoint, calibration.worldR);
|
||||
// tempPoint = RotatePoint(tempPoint, calibration.worldR);
|
||||
|
||||
tempBodies[i].vJoints[j].Position.X = tempPoint.X;
|
||||
tempBodies[i].vJoints[j].Position.Y = tempPoint.Y;
|
||||
tempBodies[i].vJoints[j].Position.Z = tempPoint.Z;
|
||||
}
|
||||
}
|
||||
}
|
||||
// tempBodies[i].vJoints[j].Position.X = tempPoint.X;
|
||||
// tempBodies[i].vJoints[j].Position.Y = tempPoint.Y;
|
||||
// tempBodies[i].vJoints[j].Position.Z = tempPoint.Z;
|
||||
// }
|
||||
// }
|
||||
//}
|
||||
|
||||
if (m_bFilter)
|
||||
filter(goodVertices, goodColorPoints, m_nFilterNeighbors, m_fFilterThreshold);
|
||||
|
||||
Reference in New Issue
Block a user