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
Debug|Win32|H:\Repositories\MasterArbeit\3_PointCloud\|
#TargetFrameworkVersion=v4.0:PlatformToolSet=v141:EnableManagedIncrementalBuild=false:VCToolArchitecture=Native32Bit:WindowsTargetPlatformVersion=8.1
Debug|Win32|D:\Dropbox\Studium\MasterArbeit\3_PointCloud\|
3_PointCloud/KinectTutorial3.vcxproj
<?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,12 +19,12 @@
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">
<ConfigurationType>Application</ConfigurationType>
<UseDebugLibraries>true</UseDebugLibraries>
<PlatformToolset>v140</PlatformToolset>
<PlatformToolset>v141</PlatformToolset>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
<ConfigurationType>Application</ConfigurationType>
<UseDebugLibraries>false</UseDebugLibraries>
<PlatformToolset>v140</PlatformToolset>
<PlatformToolset>v141</PlatformToolset>
</PropertyGroup>
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" />
<ImportGroup Label="ExtensionSettings">
......
3_PointCloud/main.cpp
......@@ -7,6 +7,12 @@
#include <Windows.h>
#include <Ole2.h>
#include <vector>
#include <iostream>
#include <fstream>
#include <sstream>
#include <Kinect.h>
// We'll be using buffer objects to store the kinect point cloud
......@@ -17,6 +23,11 @@ GLuint cboId;
unsigned char rgbimage[colorwidth*colorheight*4]; // Stores RGB color image
ColorSpacePoint depth2rgb[width*height]; // Maps depth pixels to rgb pixels
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
IKinectSensor* sensor; // Kinect sensor
......@@ -40,7 +51,7 @@ bool initKinect() {
}
}
void getDepthData(IMultiSourceFrame* frame, GLubyte* dest) {
void getDepthData(IMultiSourceFrame* frame) {
IDepthFrame* depthframe;
IDepthFrameReference* frameref = NULL;
frame->get_DepthFrameReference(&frameref);
......@@ -56,7 +67,7 @@ void getDepthData(IMultiSourceFrame* frame, GLubyte* dest) {
// Write vertex coordinates
mapper->MapDepthFrameToCameraSpace(width*height, buf, width*height, depth2xyz);
float* fdest = (float*)dest;
float* fdest = (float*)framePositions;
for (int i = 0; i < sz; i++) {
*fdest++ = depth2xyz[i].X;
*fdest++ = depth2xyz[i].Y;
......@@ -68,7 +79,7 @@ void getDepthData(IMultiSourceFrame* frame, GLubyte* dest) {
if (depthframe) depthframe->Release();
}
void getRgbData(IMultiSourceFrame* frame, GLubyte* dest) {
void getRgbData(IMultiSourceFrame* frame) {
IColorFrame* colorframe;
IColorFrameReference* frameref = NULL;
frame->get_ColorFrameReference(&frameref);
......@@ -80,21 +91,34 @@ void getRgbData(IMultiSourceFrame* frame, GLubyte* dest) {
// Get data from frame
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
float* fdest = (float*)dest;
float* fdest = (float*)frameColors;
for (int i = 0; i < width*height; i++) {
ColorSpacePoint p = depth2rgb[i];
// Check if color pixel coordinates are in bounds
if (p.X < 0 || p.Y < 0 || p.X > colorwidth || p.Y > colorheight) {
*fdest++ = 0;
*fdest++ = 0;
*fdest++ = 0;
fdest[3 * i + 0] = 1;
fdest[3 * i + 1] = 0;
fdest[3 * i + 2] = 0;
}
else {
int idx = (int)p.X + colorwidth*(int)p.Y;
*fdest++ = rgbimage[4*idx + 0]/255.;
*fdest++ = rgbimage[4*idx + 1]/255.;
*fdest++ = rgbimage[4*idx + 2]/255.;
fdest[3 * i + 0] = rgbimage[4 * idx + 0] / 255.;
fdest[3 * i + 1] = rgbimage[4 * idx + 1] / 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
}
......@@ -105,23 +129,49 @@ void getRgbData(IMultiSourceFrame* frame, GLubyte* dest) {
void getKinectData() {
IMultiSourceFrame* frame = NULL;
if (SUCCEEDED(reader->AcquireLatestFrame(&frame))) {
GLubyte* ptr;
getDepthData(frame);
getRgbData(frame);
glBindBuffer(GL_ARRAY_BUFFER, vboId);
ptr = (GLubyte*)glMapBuffer(GL_ARRAY_BUFFER, GL_WRITE_ONLY);
if (ptr) {
getDepthData(frame, ptr);
}
glBufferData(GL_ARRAY_BUFFER, width*height * 3 * sizeof(GLfloat), framePositions, GL_DYNAMIC_DRAW);
glUnmapBuffer(GL_ARRAY_BUFFER);
glBindBuffer(GL_ARRAY_BUFFER, cboId);
ptr = (GLubyte*)glMapBuffer(GL_ARRAY_BUFFER, GL_WRITE_ONLY);
if (ptr) {
getRgbData(frame, ptr);
}
glBufferData(GL_ARRAY_BUFFER, width*height * 3 * sizeof(GLfloat), frameColors, GL_DYNAMIC_DRAW);
glUnmapBuffer(GL_ARRAY_BUFFER);
}
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() {
static double angle = 0.;
static double radius = 3.;
......@@ -152,6 +202,7 @@ void drawKinectData() {
glDisableClientState(GL_VERTEX_ARRAY);
glDisableClientState(GL_COLOR_ARRAY);
writePointCloud();
}
int main(int argc, char* argv[]) {
......@@ -182,5 +233,6 @@ int main(int argc, char* argv[]) {
// Main loop
execute();
//drawKinectData();
return 0;
}
3_PointCloud/main.h
......@@ -5,3 +5,14 @@ const int colorwidth = 1920;
const int colorheight = 1080;
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