Skip to content

M
masterarbeit-kai
Commit e4c891ad by wester
Options

kinect update

parent 7bad8df5
Showing with 125 additions and 166 deletions
3_PointCloud/KinectTutorial3.vcxproj
...@@ -93,6 +93,7 @@ ...@@ -93,6 +93,7 @@
</ItemGroup> </ItemGroup>
<ItemGroup> <ItemGroup>
<ClInclude Include="glut.h" /> <ClInclude Include="glut.h" />
<ClInclude Include="SteamVrTrackingServer.h" />
<ClInclude Include="main.h" /> <ClInclude Include="main.h" />
</ItemGroup> </ItemGroup>
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" /> <Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
......
3_PointCloud/KinectTutorial3.vcxproj.filters
...@@ -32,5 +32,8 @@ ...@@ -32,5 +32,8 @@
<ClInclude Include="main.h"> <ClInclude Include="main.h">
<Filter>Header Files</Filter> <Filter>Header Files</Filter>
</ClInclude> </ClInclude>
<ClInclude Include="SteamVrTrackingServer.h">
<Filter>Header Files</Filter>
</ClInclude>
</ItemGroup> </ItemGroup>
</Project> </Project>
\ No newline at end of file
3_PointCloud/SteamVrTrackingServer.cpp
#include <chrono> #include "SteamVrTrackingServer.h"
#include <csignal>
#include <iostream>
#include <thread>
#include <sstream>
#include <openvr.h>
struct Config struct Config
{ {
std::string hostname = "127.0.0.1";
int port = 5672;
std::string exchange = "irpose";
bool debug = false; bool debug = false;
int messages_per_second = 60;
} config_; } config_;
bool running_ = true;
vr::IVRSystem *vr_system_ = nullptr; vr::IVRSystem *vr_system_ = nullptr;
vr::TrackedDevicePose_t tracked_device_poses_[vr::k_unMaxTrackedDeviceCount]; vr::TrackedDevicePose_t tracked_device_poses_[vr::k_unMaxTrackedDeviceCount];
void signalHandler(int signum)
{
running_ = false;
}
bool init() bool initVR()
{ {
// Loading the SteamVR Runtime // Loading the SteamVR Runtime
...@@ -57,7 +44,7 @@ bool init() ...@@ -57,7 +44,7 @@ bool init()
return true; return true;
} }
void shutdown() void shutdownVR()
{ {
if (vr_system_) if (vr_system_)
{ {
...@@ -134,162 +121,107 @@ void printMatrix(const vr::HmdMatrix34_t &htm) ...@@ -134,162 +121,107 @@ void printMatrix(const vr::HmdMatrix34_t &htm)
} }
} }
int main2(int argc, char *argv[]) void printVRDevices() {
{ std::cout << "List of available VR devices:" << std::endl;
if (!init()) for (int nDevice = 0; nDevice < vr::k_unMaxTrackedDeviceCount; ++nDevice)
{
shutdown();
return 1;
}
if (false)
{ {
std::cout << "List of available VR devices:" << std::endl; vr::ETrackedDeviceClass device_class = vr_system_->GetTrackedDeviceClass(nDevice);
for (int nDevice = 0; nDevice < vr::k_unMaxTrackedDeviceCount; ++nDevice) if (device_class == vr::TrackedDeviceClass_Invalid)
{ {
vr::ETrackedDeviceClass device_class = vr_system_->GetTrackedDeviceClass(nDevice); continue;
if (device_class == vr::TrackedDeviceClass_Invalid)
{
continue;
}
std::string type;
switch (device_class)
{
case vr::TrackedDeviceClass_Controller: type = "Controller"; break;
case vr::TrackedDeviceClass_HMD: type = "HMD"; break;
case vr::TrackedDeviceClass_Invalid: type = "Invalid"; break;
case vr::TrackedDeviceClass_GenericTracker: type = "GenericTracker"; break;
case vr::TrackedDeviceClass_TrackingReference: type = "TrackingReference"; break;
default: type = "Unknown"; break;
}
const uint32_t size = 1024;
char serial[size];
vr_system_->GetStringTrackedDeviceProperty(nDevice, vr::ETrackedDeviceProperty::Prop_SerialNumber_String, serial, size);
std::cout << "\tDevice " << nDevice << ":" << std::endl;
std::cout << "\t\tType: " << type << std::endl;
std::cout << "\t\tSerial: " << serial << std::endl;
} }
shutdown(); std::string type;
return 0; switch (device_class)
} {
case vr::TrackedDeviceClass_Controller: type = "Controller"; break;
case vr::TrackedDeviceClass_HMD: type = "HMD"; break;
case vr::TrackedDeviceClass_Invalid: type = "Invalid"; break;
case vr::TrackedDeviceClass_GenericTracker: type = "GenericTracker"; break;
case vr::TrackedDeviceClass_TrackingReference: type = "TrackingReference"; break;
default: type = "Unknown"; break;
}
signal(SIGINT, signalHandler); const uint32_t size = 1024;
std::cout << "Tracking started. Cancel with Ctrl-C" << std::endl; char serial[size];
vr_system_->GetStringTrackedDeviceProperty(nDevice, vr::ETrackedDeviceProperty::Prop_SerialNumber_String, serial, size);
std::chrono::duration<double, std::milli> cycle_time; std::cout << "\tDevice " << nDevice << ":" << std::endl;
if (config_.messages_per_second > 0) std::cout << "\t\tType: " << type << std::endl;
{ std::cout << "\t\tSerial: " << serial << std::endl;
cycle_time = std::chrono::duration<double, std::milli>(1000.0 / config_.messages_per_second);
} }
}
while (running_) int main2(int argc, char *argv[])
{
if (!initVR())
{ {
auto start = std::chrono::steady_clock::now(); shutdownVR();
return 1;
}
printVRDevices();
handleInput(); handleInput();
// Get device poses // Get device poses
vr_system_->GetDeviceToAbsoluteTrackingPose(vr::TrackingUniverseRawAndUncalibrated, 0, tracked_device_poses_, vr::k_unMaxTrackedDeviceCount); vr_system_->GetDeviceToAbsoluteTrackingPose(vr::TrackingUniverseRawAndUncalibrated, 0, tracked_device_poses_, vr::k_unMaxTrackedDeviceCount);
// TODO: remove // TODO: remove
vr::HmdMatrix34_t raw_to_standing_htm = vr_system_->GetRawZeroPoseToStandingAbsoluteTrackingPose(); vr::HmdMatrix34_t raw_to_standing_htm = vr_system_->GetRawZeroPoseToStandingAbsoluteTrackingPose();
std::cout << "RawZeroPoseToStandingAbsoluteTrackingPose:" << std::endl; std::cout << "RawZeroPoseToStandingAbsoluteTrackingPose:" << std::endl;
printMatrix(raw_to_standing_htm); printMatrix(raw_to_standing_htm);
for (int nDevice = 0; nDevice < vr::k_unMaxTrackedDeviceCount; ++nDevice) for (int nDevice = 0; nDevice < vr::k_unMaxTrackedDeviceCount; ++nDevice)
{
if (!tracked_device_poses_[nDevice].bPoseIsValid)
{ {
if (!tracked_device_poses_[nDevice].bPoseIsValid) continue;
{ }
continue;
}
vr::ETrackedDeviceClass device_class = vr_system_->GetTrackedDeviceClass(nDevice);
if (device_class == vr::TrackedDeviceClass_Invalid)
{
continue;
}
vr::HmdMatrix34_t htm = tracked_device_poses_[nDevice].mDeviceToAbsoluteTracking; vr::ETrackedDeviceClass device_class = vr_system_->GetTrackedDeviceClass(nDevice);
std::string type; if (device_class == vr::TrackedDeviceClass_Invalid)
std::string model_type; {
switch (device_class) continue;
{ }
case vr::TrackedDeviceClass_Controller:
type = "Controller";
model_type = "vive_controller";
break;
case vr::TrackedDeviceClass_HMD:
type = "HMD";
model_type = "vive_hmd";
break;
case vr::TrackedDeviceClass_Invalid:
type = "Invalid";
break;
case vr::TrackedDeviceClass_GenericTracker:
type = "GenericTracker";
break;
case vr::TrackedDeviceClass_TrackingReference:
type = "TrackingReference";
model_type = "vive_base_station";
break;
default:
type = "Unknown";
break;
}
if (config_.debug) vr::HmdMatrix34_t htm = tracked_device_poses_[nDevice].mDeviceToAbsoluteTracking;
{
std::cout << "Pose of device " << nDevice << " (" << type << ")" << std::endl;
printMatrix(htm);
}
const uint32_t size = 1024; std::string type;
char serial[size]; std::string model_type;
vr_system_->GetStringTrackedDeviceProperty(nDevice, vr::ETrackedDeviceProperty::Prop_SerialNumber_String, serial, size); switch (device_class)
{
auto timestamp = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::system_clock::now().time_since_epoch()).count(); case vr::TrackedDeviceClass_Controller:
type = "Controller";
std::ostringstream message; model_type = "vive_controller";
message << "%YAML:1.0\n"; break;
message << "object_type: \"" << model_type << "\"\n"; case vr::TrackedDeviceClass_HMD:
message << "serial: \"" << serial << "\"\n"; type = "HMD";
message << "timestamp: \"" << std::to_string(timestamp) << "\"\n"; model_type = "vive_hmd";
message << "pose: !!opencv-matrix\n rows: 4\n cols: 4\n dt: f\n data: [ "; break;
message << htm.m[0][0] << ", "; case vr::TrackedDeviceClass_Invalid:
message << htm.m[0][1] << ", "; type = "Invalid";
message << htm.m[0][2] << ", "; break;
message << htm.m[0][3] << ", "; case vr::TrackedDeviceClass_GenericTracker:
message << htm.m[1][0] << ", "; type = "GenericTracker";
message << htm.m[1][1] << ", "; break;
message << htm.m[1][2] << ", "; case vr::TrackedDeviceClass_TrackingReference:
message << htm.m[1][3] << ", "; type = "TrackingReference";
message << htm.m[2][0] << ", "; model_type = "vive_base_station";
message << htm.m[2][1] << ", "; break;
message << htm.m[2][2] << ", "; default:
message << htm.m[2][3] << ", "; type = "Unknown";
message << "0.0, 0.0, 0.0, 1.0 ]\n"; break;
message << "source_type: \"steam_vr\"\n";
message << "source_serial: \"steam_vr_1\"\n";
if (config_.debug)
{
std::cout << message.str() << std::endl;
}
} }
if (config_.messages_per_second > 0) if (config_.debug)
{ {
std::this_thread::sleep_until(start + cycle_time); std::cout << "Pose of device " << nDevice << " (" << type << ")" << std::endl;
printMatrix(htm);
} }
} }
shutdown(); shutdownVR();
return 0; return 0;
} }
3_PointCloud/SteamVrTrackingServer.h 0 → 100644
#pragma once
#include <iostream>
#include <sstream>
#include <openvr.h>
bool initVR();
void shutdownVR();
\ No newline at end of file
3_PointCloud/main.cpp
#include "main.h" #include "main.h"
#include "glut.h" #include "glut.h"
#include "SteamVRTrackingServer.h"
#include <cmath> #include <cmath>
#include <cstdio> #include <cstdio>
...@@ -92,13 +93,6 @@ void getRgbData(IMultiSourceFrame* frame, bool writFrameToFile) { ...@@ -92,13 +93,6 @@ void getRgbData(IMultiSourceFrame* frame, bool writFrameToFile) {
// Get data from frame // Get data from frame
colorframe->CopyConvertedFrameDataToArray(colorwidth*colorheight*4, rgbimage, ColorImageFormat_Rgba); colorframe->CopyConvertedFrameDataToArray(colorwidth*colorheight*4, rgbimage, ColorImageFormat_Rgba);
if (writFrameToFile) {
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*)frameColors; float* fdest = (float*)frameColors;
...@@ -132,13 +126,19 @@ void getRgbData(IMultiSourceFrame* frame, bool writFrameToFile) { ...@@ -132,13 +126,19 @@ void getRgbData(IMultiSourceFrame* frame, bool writFrameToFile) {
if (colorframe) colorframe->Release(); if (colorframe) colorframe->Release();
} }
void getKinectData() { void getKinectData(bool writeFrameInVector) {
IMultiSourceFrame* frame = NULL; IMultiSourceFrame* frame = NULL;
if (SUCCEEDED(reader->AcquireLatestFrame(&frame))) { if (SUCCEEDED(reader->AcquireLatestFrame(&frame))) {
getDepthData(frame);
getRgbData(frame, writeFile);
writeFile = false;
if (writeFrameInVector) {
points.clear();
points.reserve(width*height * 3);
colors.clear();
colors.reserve(width*height * 3);
}
getDepthData(frame);
getRgbData(frame, writeFrameInVector);
glBindBuffer(GL_ARRAY_BUFFER, vboId); glBindBuffer(GL_ARRAY_BUFFER, vboId);
glBufferData(GL_ARRAY_BUFFER, width*height * 3 * sizeof(GLfloat), framePositions, GL_DYNAMIC_DRAW); glBufferData(GL_ARRAY_BUFFER, width*height * 3 * sizeof(GLfloat), framePositions, GL_DYNAMIC_DRAW);
...@@ -178,8 +178,6 @@ void writePointCloud() { ...@@ -178,8 +178,6 @@ void writePointCloud() {
myfile.close(); myfile.close();
} }
void rotateCamera() { void rotateCamera() {
...@@ -190,11 +188,17 @@ void rotateCamera() { ...@@ -190,11 +188,17 @@ void rotateCamera() {
glMatrixMode(GL_MODELVIEW); glMatrixMode(GL_MODELVIEW);
glLoadIdentity(); glLoadIdentity();
gluLookAt(x,0,z,0,0,radius/2,0,1,0); gluLookAt(x,0,z,0,0,radius/2,0,1,0);
angle += 0.002; //angle += 0.002;
} }
void drawKinectData() { void drawKinectData() {
getKinectData();
getKinectData(writeFile);
if (writeFile) {
writePointCloud();
writeFile = false;
}
rotateCamera(); rotateCamera();
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
...@@ -212,7 +216,7 @@ void drawKinectData() { ...@@ -212,7 +216,7 @@ void drawKinectData() {
glDisableClientState(GL_VERTEX_ARRAY); glDisableClientState(GL_VERTEX_ARRAY);
glDisableClientState(GL_COLOR_ARRAY); glDisableClientState(GL_COLOR_ARRAY);
writePointCloud();
} }
void keyPressed(unsigned char key, int x, int y) void keyPressed(unsigned char key, int x, int y)
...@@ -232,6 +236,12 @@ void keyPressed(unsigned char key, int x, int y) ...@@ -232,6 +236,12 @@ void keyPressed(unsigned char key, int x, int y)
int main(int argc, char* argv[]) { int main(int argc, char* argv[]) {
if (!init(argc, argv)) return 1; if (!init(argc, argv)) return 1;
if (!initKinect()) return 1; if (!initKinect()) return 1;
if (!initVR())
{
shutdownVR();
}
// OpenGL setup // OpenGL setup
glClearColor(0,0,0,0); glClearColor(0,0,0,0);
glClearDepth(1.0f); glClearDepth(1.0f);
......
3_PointCloud/main.h
...@@ -6,6 +6,7 @@ const int colorheight = 1080; ...@@ -6,6 +6,7 @@ const int colorheight = 1080;
void drawKinectData(); void drawKinectData();
void keyPressed(unsigned char key, int x, int y); void keyPressed(unsigned char key, int x, int y);
void writePointCloud();
struct pntsHeader { struct pntsHeader {
......
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