Skip to content

M
masterarbeit-kai
Commit d5562282 by Kai Westerkamp
Options

write point cloud

parent bfecb28e
Showing with 88 additions and 24 deletions
3_PointCloud/Debug/KinectTutorial3.tlog/KinectTutorial3.lastbuildstate
#TargetFrameworkVersion=v4.0:PlatformToolSet=v140:EnableManagedIncrementalBuild=false:VCToolArchitecture=Native32Bit:WindowsTargetPlatformVersion=8.1 #TargetFrameworkVersion=v4.0:PlatformToolSet=v141:EnableManagedIncrementalBuild=false:VCToolArchitecture=Native32Bit:WindowsTargetPlatformVersion=8.1
Debug|Win32|H:\Repositories\MasterArbeit\3_PointCloud\| Debug|Win32|D:\Dropbox\Studium\MasterArbeit\3_PointCloud\|
3_PointCloud/KinectTutorial3.vcxproj
<?xml version="1.0" encoding="utf-8"?> <?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"> <ItemGroup Label="ProjectConfigurations">
<ProjectConfiguration Include="Debug|Win32"> <ProjectConfiguration Include="Debug|Win32">
<Configuration>Debug</Configuration> <Configuration>Debug</Configuration>
...@@ -19,12 +19,12 @@ ...@@ -19,12 +19,12 @@
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration"> <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">
<ConfigurationType>Application</ConfigurationType> <ConfigurationType>Application</ConfigurationType>
<UseDebugLibraries>true</UseDebugLibraries> <UseDebugLibraries>true</UseDebugLibraries>
<PlatformToolset>v140</PlatformToolset> <PlatformToolset>v141</PlatformToolset>
</PropertyGroup> </PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration"> <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
<ConfigurationType>Application</ConfigurationType> <ConfigurationType>Application</ConfigurationType>
<UseDebugLibraries>false</UseDebugLibraries> <UseDebugLibraries>false</UseDebugLibraries>
<PlatformToolset>v140</PlatformToolset> <PlatformToolset>v141</PlatformToolset>
</PropertyGroup> </PropertyGroup>
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" /> <Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" />
<ImportGroup Label="ExtensionSettings"> <ImportGroup Label="ExtensionSettings">
......
3_PointCloud/main.cpp
...@@ -7,6 +7,12 @@ ...@@ -7,6 +7,12 @@
#include <Windows.h> #include <Windows.h>
#include <Ole2.h> #include <Ole2.h>
#include <vector>
#include <iostream>
#include <fstream>
#include <sstream>
#include <Kinect.h> #include <Kinect.h>
// We'll be using buffer objects to store the kinect point cloud // We'll be using buffer objects to store the kinect point cloud
...@@ -17,6 +23,11 @@ GLuint cboId; ...@@ -17,6 +23,11 @@ GLuint cboId;
unsigned char rgbimage[colorwidth*colorheight*4]; // Stores RGB color image unsigned char rgbimage[colorwidth*colorheight*4]; // Stores RGB color image
ColorSpacePoint depth2rgb[width*height]; // Maps depth pixels to rgb pixels ColorSpacePoint depth2rgb[width*height]; // Maps depth pixels to rgb pixels
CameraSpacePoint depth2xyz[width*height]; // Maps depth pixels to 3d coordinates CameraSpacePoint depth2xyz[width*height]; // Maps depth pixels to 3d coordinates
float framePositions[width*height * 3];
float frameColors[width*height * 3];
std::vector<float> points;
std::vector<float> colors;
// Kinect Variables // Kinect Variables
IKinectSensor* sensor; // Kinect sensor IKinectSensor* sensor; // Kinect sensor
...@@ -40,7 +51,7 @@ bool initKinect() { ...@@ -40,7 +51,7 @@ bool initKinect() {
} }
} }
void getDepthData(IMultiSourceFrame* frame, GLubyte* dest) { void getDepthData(IMultiSourceFrame* frame) {
IDepthFrame* depthframe; IDepthFrame* depthframe;
IDepthFrameReference* frameref = NULL; IDepthFrameReference* frameref = NULL;
frame->get_DepthFrameReference(&frameref); frame->get_DepthFrameReference(&frameref);
...@@ -56,7 +67,7 @@ void getDepthData(IMultiSourceFrame* frame, GLubyte* dest) { ...@@ -56,7 +67,7 @@ void getDepthData(IMultiSourceFrame* frame, GLubyte* dest) {
// Write vertex coordinates // Write vertex coordinates
mapper->MapDepthFrameToCameraSpace(width*height, buf, width*height, depth2xyz); mapper->MapDepthFrameToCameraSpace(width*height, buf, width*height, depth2xyz);
float* fdest = (float*)dest; float* fdest = (float*)framePositions;
for (int i = 0; i < sz; i++) { for (int i = 0; i < sz; i++) {
*fdest++ = depth2xyz[i].X; *fdest++ = depth2xyz[i].X;
*fdest++ = depth2xyz[i].Y; *fdest++ = depth2xyz[i].Y;
...@@ -68,7 +79,7 @@ void getDepthData(IMultiSourceFrame* frame, GLubyte* dest) { ...@@ -68,7 +79,7 @@ void getDepthData(IMultiSourceFrame* frame, GLubyte* dest) {
if (depthframe) depthframe->Release(); if (depthframe) depthframe->Release();
} }
void getRgbData(IMultiSourceFrame* frame, GLubyte* dest) { void getRgbData(IMultiSourceFrame* frame) {
IColorFrame* colorframe; IColorFrame* colorframe;
IColorFrameReference* frameref = NULL; IColorFrameReference* frameref = NULL;
frame->get_ColorFrameReference(&frameref); frame->get_ColorFrameReference(&frameref);
...@@ -80,21 +91,34 @@ void getRgbData(IMultiSourceFrame* frame, GLubyte* dest) { ...@@ -80,21 +91,34 @@ void getRgbData(IMultiSourceFrame* frame, GLubyte* dest) {
// Get data from frame // Get data from frame
colorframe->CopyConvertedFrameDataToArray(colorwidth*colorheight*4, rgbimage, ColorImageFormat_Rgba); colorframe->CopyConvertedFrameDataToArray(colorwidth*colorheight*4, rgbimage, ColorImageFormat_Rgba);
points.clear();
points.reserve(width*height * 3);
colors.clear();
colors.reserve(width*height * 3);
// Write color array for vertices // Write color array for vertices
float* fdest = (float*)dest; float* fdest = (float*)frameColors;
for (int i = 0; i < width*height; i++) { for (int i = 0; i < width*height; i++) {
ColorSpacePoint p = depth2rgb[i]; ColorSpacePoint p = depth2rgb[i];
// Check if color pixel coordinates are in bounds // Check if color pixel coordinates are in bounds
if (p.X < 0 || p.Y < 0 || p.X > colorwidth || p.Y > colorheight) { if (p.X < 0 || p.Y < 0 || p.X > colorwidth || p.Y > colorheight) {
*fdest++ = 0; fdest[3 * i + 0] = 1;
*fdest++ = 0; fdest[3 * i + 1] = 0;
*fdest++ = 0; fdest[3 * i + 2] = 0;
} }
else { else {
int idx = (int)p.X + colorwidth*(int)p.Y; int idx = (int)p.X + colorwidth*(int)p.Y;
*fdest++ = rgbimage[4*idx + 0]/255.; fdest[3 * i + 0] = rgbimage[4 * idx + 0] / 255.;
*fdest++ = rgbimage[4*idx + 1]/255.; fdest[3 * i + 1] = rgbimage[4 * idx + 1] / 255.;
*fdest++ = rgbimage[4*idx + 2]/255.; fdest[3 * i + 2] = rgbimage[4 * idx + 2] / 255.;
points.push_back(framePositions[3 * i + 0]);
points.push_back(framePositions[3 * i + 1]);
points.push_back(framePositions[3 * i + 2]);
colors.push_back(fdest[3 * i + 0]);
colors.push_back(fdest[3 * i + 1]);
colors.push_back(fdest[3 * i + 2]);
} }
// Don't copy alpha channel // Don't copy alpha channel
} }
...@@ -105,23 +129,49 @@ void getRgbData(IMultiSourceFrame* frame, GLubyte* dest) { ...@@ -105,23 +129,49 @@ void getRgbData(IMultiSourceFrame* frame, GLubyte* dest) {
void getKinectData() { void getKinectData() {
IMultiSourceFrame* frame = NULL; IMultiSourceFrame* frame = NULL;
if (SUCCEEDED(reader->AcquireLatestFrame(&frame))) { if (SUCCEEDED(reader->AcquireLatestFrame(&frame))) {
GLubyte* ptr; getDepthData(frame);
getRgbData(frame);
glBindBuffer(GL_ARRAY_BUFFER, vboId); glBindBuffer(GL_ARRAY_BUFFER, vboId);
ptr = (GLubyte*)glMapBuffer(GL_ARRAY_BUFFER, GL_WRITE_ONLY); glBufferData(GL_ARRAY_BUFFER, width*height * 3 * sizeof(GLfloat), framePositions, GL_DYNAMIC_DRAW);
if (ptr) {
getDepthData(frame, ptr);
}
glUnmapBuffer(GL_ARRAY_BUFFER); glUnmapBuffer(GL_ARRAY_BUFFER);
glBindBuffer(GL_ARRAY_BUFFER, cboId); glBindBuffer(GL_ARRAY_BUFFER, cboId);
ptr = (GLubyte*)glMapBuffer(GL_ARRAY_BUFFER, GL_WRITE_ONLY); glBufferData(GL_ARRAY_BUFFER, width*height * 3 * sizeof(GLfloat), frameColors, GL_DYNAMIC_DRAW);
if (ptr) {
getRgbData(frame, ptr);
}
glUnmapBuffer(GL_ARRAY_BUFFER); glUnmapBuffer(GL_ARRAY_BUFFER);
} }
if (frame) frame->Release(); if (frame) frame->Release();
} }
void writePointCloud() {
pntsHeader header;
size_t arrayByteSize = points.size() * 3 * sizeof(float);
header.featureTableByteLenght = 2 * arrayByteSize;
std::ostringstream os;
os << "{\"POINTS_LENGTH\":" << points.size() << ", \"POSITION\" : {\"byteOffset \":0}, \"RGB\" : {\"byteOffset\":" << arrayByteSize << "}}";
std::string json = os.str();
header.byteLenght = 28 /*header*/ + json.size() + arrayByteSize;
std::cout <<"File Size " << header.byteLenght;
std::ofstream myfile;
myfile.open("example.pnts", std::ios::out | std::ios::binary);
myfile.write((char*)&header, 28);
myfile << json;
myfile.write((char*)&points, 28);
myfile.write((char*)&colors, 28);
//myfile <<
myfile.close();
}
void rotateCamera() { void rotateCamera() {
static double angle = 0.; static double angle = 0.;
static double radius = 3.; static double radius = 3.;
...@@ -152,6 +202,7 @@ void drawKinectData() { ...@@ -152,6 +202,7 @@ void drawKinectData() {
glDisableClientState(GL_VERTEX_ARRAY); glDisableClientState(GL_VERTEX_ARRAY);
glDisableClientState(GL_COLOR_ARRAY); glDisableClientState(GL_COLOR_ARRAY);
writePointCloud();
} }
int main(int argc, char* argv[]) { int main(int argc, char* argv[]) {
...@@ -182,5 +233,6 @@ int main(int argc, char* argv[]) { ...@@ -182,5 +233,6 @@ int main(int argc, char* argv[]) {
// Main loop // Main loop
execute(); execute();
//drawKinectData();
return 0; return 0;
} }
3_PointCloud/main.h
...@@ -5,3 +5,14 @@ const int colorwidth = 1920; ...@@ -5,3 +5,14 @@ const int colorwidth = 1920;
const int colorheight = 1080; const int colorheight = 1080;
void drawKinectData(); void drawKinectData();
struct pntsHeader {
unsigned char magic[4] = { 'p','n','t','s' };
unsigned int version = 1;
unsigned int byteLenght = 28;
unsigned int featureTableJSONByteLenght = 0;
unsigned int featureTableByteLenght = 0;
unsigned int batchTableJSONByteLength = 0;
unsigned int batchTableBinaryLenght = 0;
};
\ No newline at end of file
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment