Im not an expert in C++ or Python so some of the code can probably be fixed to make it alittle more streamlined, but it works.
Im going to post the complete code since im unsure how to add attachments atm.
Creating the DLL using Microsoft Visual C++ 2010 Express
New Project
- Win32 Console Application
- - Application Type - DLL
First things First:
Adding libraries
- Project Properties
- - Configuration Properties -> C/C++ -> General -> Additional Include Directories
- - - OptiTrack Camera SDK\include path
- - Configuration Properties -> C/C++ -> Preprocessor -> Preprocessor Definitions
- - - Add CAMERALIBRARY_IMPORTS to the bottom of the list
- - Configuration Properties -> Linker -> General -> Additional Library Directories
- - - OptiTrack Camera SDK\lib path
- - Configuration Properties -> Linker -> Input -> Additional Dependencies
- - - Add cameralibrary.lib
Add cameralibrary.dll into the same folder as index.py (cameralibrary.dll not included)
The dll returns a 4x4 view matrix that can be multiplied right into a GL_MODELVIEW (or equivalent)
It also adds some basic smoothing to get rid of jittering although this can be removed
stdaf.h - Since precompiled headers are on automatically in VC i just used this cause it was there
[spoiler]
Code: Select all
// stdafx.h : include file for standard system include files,
// or project specific include files that are used frequently, but
// are changed infrequently
//
#pragma once
#include "targetver.h"
#define WIN32_LEAN_AND_MEAN // Exclude rarely-used stuff from Windows headers
// Windows Header Files:
#include <windows.h>
// TODO: reference additional headers your program requires here
#if _MSC_VER // this is defined when compiling with Visual Studio
#define EXPORT __declspec(dllexport) // Visual Studio needs annotating exported functions with this
#else
#define EXPORT // XCode does not need annotating exported functions, so define is empty
#endif
#include "cameralibrary.h"
extern "C"
{
EXPORT void wSetupCamera(void);
EXPORT bool wAreCamerasInitialized(void);
EXPORT void wShutdownCamera(void);
EXPORT bool wStartCamera(void);
EXPORT bool wStopCamera(void);
EXPORT void wRecenter(void);
EXPORT float* wGetMatrix(void);
void setCameraData(void);
float* ResetIdentity(void);
float degToRad(float);
void quatCreateFromAxisAngle(float*, int[], float);
void quatMultiply(float*, float[]);
void quatToMat4(float*, float[]);
void updateBuffer(double*, double, double*, double, double*, double);
void smoothed(double*, double*, double*, double*, double*, double*);
}
Code: Select all
// TrackIR.cpp : Defines the exported functions for the DLL application.
//
#include "stdafx.h"
CameraLibrary::CameraManager *manager=0;
CameraLibrary::Camera *gCamera=0;
CameraLibrary::Frame *gFrame=0;
CameraLibrary::cModuleVector *vec = CameraLibrary::cModuleVector::Create(); //new cModuleVector();
CameraLibrary::cModuleVectorProcessing *vecprocessor = new CameraLibrary::cModuleVectorProcessing();
Core::DistortionModel lensDistortion;
int bufferSize = 10;
EXPORT void wSetupCamera() {
manager = &CameraLibrary::CameraManager::X();
manager->WaitForInitialization();
}
EXPORT bool wAreCamerasInitialized() {
return manager->AreCamerasInitialized();
}
EXPORT void wShutdownCamera() {
if(manager) {
if(manager->AreCamerasInitialized()) {
manager->Shutdown();
manager = 0;
}
}
}
EXPORT bool wStartCamera() {
gCamera = manager->GetCamera();
if(gCamera) {
gCamera->SetVideoType(CameraLibrary::SegmentMode);
//gCamera->SetTextOverlay(false);
//gCamera->SetIntensity(5);
//gCamera->SetExposure(3);
//gCamera->SetThreshold(200);
gCamera->GetDistortionModel(lensDistortion);
gCamera->Start();
setCameraData();
return true;
} else {
return false;
}
}
EXPORT bool wStopCamera() {
if(gCamera) {
gCamera->Stop();
gCamera->Release();
}
return true;
}
EXPORT void wRecenter(void) {
vecprocessor->Recenter();
}
EXPORT float* wGetMatrix() {
static float* matrixData = new float[16];
matrixData = ResetIdentity();
static double yawBuffer[10];
static double pitchBuffer[10];
static double rollBuffer[10];
CameraLibrary::Frame *frame = gCamera->GetLatestFrame();
if(frame!=0) {
if(frame->ObjectCount() >= 3) {
float x = 0;
float y = 0;
vec->BeginFrame();
for(int i=0; i<frame->ObjectCount(); i++) {
CameraLibrary::cObject *obj = frame->Object(i);
x = obj->X();
y = obj->Y();
Core::Predistort2DPoint(lensDistortion,x,y);
vec->PushMarkerData(x, y, obj->Area(), obj->Width(), obj->Height());
}
vec->Calculate();
vecprocessor->PushData(vec);
if(vecprocessor->MarkerCount()>0) {
double x,y,z;
double yaw,pitch,roll;
vecprocessor->GetPosition(x,y,z);
vecprocessor->GetOrientation(yaw,pitch,roll);
if(yaw == 0.0 && pitch == 0.0 && roll == 0.0) {
} else {
updateBuffer(yawBuffer, yaw, pitchBuffer, pitch, rollBuffer, roll);
}
smoothed(yawBuffer, &yaw, pitchBuffer, &pitch, rollBuffer, &roll);
float quat[] = { 0.0f, 0.0f, 0.0f, 1.0f };
float qYaw[] = { 0.0f, 0.0f, 0.0f, 1.0f };
float qPitch[] = { 0.0f, 0.0f, 0.0f, 1.0f };
float qRoll[] = { 0.0f, 0.0f, 0.0f, 1.0f };
int oYaw[] = { 0, 1, 0 };
int oPitch[] = { 1, 0, 0 };
int oRoll[] = { 0, 0, 1 };
quatCreateFromAxisAngle(qYaw, oYaw, degToRad((float)yaw));
quatCreateFromAxisAngle(qPitch, oPitch, degToRad((float)pitch));
quatCreateFromAxisAngle(qRoll, oRoll, degToRad((float)roll));
quatMultiply(quat, qYaw);
quatMultiply(quat, qPitch);
quatMultiply(quat, qRoll);
quatToMat4(matrixData, quat);
matrixData[12] = (float)x;
matrixData[13] = (float)y;
matrixData[14] = (float)z;
}
}
frame->Release();
}
return matrixData;
}
float* ResetIdentity() {
float* identity = new float[16];
identity[0] = 1.0f; identity[1] = 0.0f; identity[2] = 0.0f; identity[3] = 0.0f;
identity[4] = 0.0f; identity[5] = 1.0f; identity[6] = 0.0f; identity[7] = 0.0f;
identity[8] = 0.0f; identity[9] = 0.0f; identity[10] = 1.0f; identity[11] = 0.0f;
identity[12] = 0.0f; identity[13] = 0.0f; identity[14] = 0.0f; identity[15] = 1.0f;
return identity;
}
void setCameraData() {
vec = CameraLibrary::cModuleVector::Create(); //new cModuleVector();
vecprocessor = new CameraLibrary::cModuleVectorProcessing();
CameraLibrary::cVectorSettings vectorSettings;
vectorSettings = *vec->Settings();
vectorSettings.Arrangement = CameraLibrary::cVectorSettings::VectorClip;
//vectorSettings.Arrangement = CameraLibrary::cVectorSettings::TrackClipPro;
vectorSettings.Enabled = true;
CameraLibrary::cVectorProcessingSettings vectorProcessorSettings;
vectorProcessorSettings = *vecprocessor->Settings();
vectorProcessorSettings.Arrangement = CameraLibrary::cVectorSettings::VectorClip;
//vectorProcessorSettings.Arrangement = CameraLibrary::cVectorSettings::TrackClipPro;
vectorProcessorSettings.ShowPivotPoint = false;
vectorProcessorSettings.ShowProcessed = false;
vecprocessor->SetSettings(vectorProcessorSettings);
vectorSettings.ImagerFocalLength = (lensDistortion.HorizontalFocalLength/((float) gCamera->PhysicalPixelWidth()))*gCamera->ImagerWidth();
vectorSettings.ImagerHeight = gCamera->ImagerHeight();
vectorSettings.ImagerWidth = gCamera->ImagerWidth();
vectorSettings.PrincipalX = gCamera->PhysicalPixelWidth()/2;
vectorSettings.PrincipalY = gCamera->PhysicalPixelHeight()/2;
vectorSettings.PixelWidth = gCamera->PhysicalPixelWidth();
vectorSettings.PixelHeight = gCamera->PhysicalPixelHeight();
vec->SetSettings(vectorSettings);
}
float degToRad(float degree) {
return degree * 3.142f / 180;
}
void quatCreateFromAxisAngle(float* quat, int order[], float degree) {
float r = sin(degree / 2);
quat[0] = order[0] * r;
quat[1] = order[1] * r;
quat[2] = order[2] * r;
quat[3] = cos(degree / 2);
}
void quatMultiply(float* quat1, float quat2[]) {
float d,e,g,a,f,h,i,b;
d = quat1[0];
e = quat1[1];
g = quat1[2];
a = quat1[3];
f = quat2[0];
h = quat2[1];
i = quat2[2];
b = quat2[3];
quat1[0] = d*b+a*f+e*i-g*h;
quat1[1] = e*b+a*h+g*f-d*i;
quat1[2] = g*b+a*i+d*h-e*f;
quat1[3] = a*b-d*f-e*h-g*i;
}
void quatToMat4(float* matrixData, float quat[]) {
//matrixData = ResetIdentity();
float c, d, e, g, f, h, i, j, k, l;
c = quat[0];
d = quat[1];
e = quat[2];
g = quat[3];
f = c+c;
h = d+d;
i = e+e;
j = c*f;
k = c*h;
c = c*i;
l = d*h;
d = d*i;
e = e*i;
f = g*f;
h = g*h;
g = g*i;
matrixData[0] = 1-(l+e);
matrixData[1] = k-g;
matrixData[2] = c+h;
matrixData[3] = 0;
matrixData[4] = k+g;
matrixData[5] = 1-(j+e);
matrixData[6] = d-f;
matrixData[7] = 0;
matrixData[8] = c-h;
matrixData[9] = d+f;
matrixData[10] = 1-(j+l);
matrixData[11] = 0;
matrixData[12] = 0;
matrixData[13] = 0;
matrixData[14] = 0;
matrixData[15] = 1;
}
void updateBuffer(double* yawBuffer, double yaw, double* pitchBuffer, double pitch, double* rollBuffer, double roll) {
for(int i = 0; i < (bufferSize-1); i++) {
yawBuffer[i] = yawBuffer[i+1];
pitchBuffer[i] = pitchBuffer[i+1];
rollBuffer[i] = rollBuffer[i+1];
}
yawBuffer[(bufferSize-1)] = yaw;
pitchBuffer[(bufferSize-1)] = pitch;
rollBuffer[(bufferSize-1)] = roll;
}
void smoothed(double* yawBuffer, double* yaw, double* pitchBuffer, double* pitch, double* rollBuffer, double* roll) {
double totalYaw = 0, totalPitch = 0, totalRoll = 0;
for(int i = 0; i < bufferSize; i++) {
totalYaw += yawBuffer[i];
totalPitch += pitchBuffer[i];
totalRoll += rollBuffer[i];
}
*yaw = totalYaw / bufferSize;
*pitch = totalPitch / bufferSize;
*roll = totalRoll / bufferSize;
}
Code: Select all
import OpenGL
OpenGL.ERROR_ON_COPY = True
from OpenGL.GL import *
from OpenGL.GLU import *
from OpenGL.GLUT import *
from OpenGL.arrays import vbo
from OpenGL.GL.shaders import *
from ctypes import *
import numpy
global haveCamera
haveCamera = False
trackIRMatrix = [1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1]
trackIRMatrix = numpy.array(trackIRMatrix, numpy.float32)
program = None
coordBuffer = []
trackir = windll.LoadLibrary("./Debug/trackir.dll")
trackirSetupCamera = getattr(trackir,"wSetupCamera")
trackirAreCamerasInitialized = getattr(trackir,"wAreCamerasInitialized")
trackirShutdownCamera = getattr(trackir,"wShutdownCamera")
trackirStartCamera = getattr(trackir,"wStartCamera")
trackirStopCamera = getattr(trackir,"wStopCamera")
trackirRecenter = getattr(trackir,"wRecenter")
trackirGetMatrix = getattr(trackir,"wGetMatrix")
trackirHandle = trackir._handle
def deg2rad(degrees):
return degrees * math.pi / 180
def rad2deg(radians):
return radians * 180 / math.pi
def InitGL(Width, Height): # We call this right after our OpenGL window is created.
if not glUseProgram:
print 'Missing Shader Objects!'
sys.exit(1)
global program
program = compileProgram(
compileShader('''
#version 120
attribute vec3 position;
attribute vec4 aColor;
varying vec4 vColor;
void main()
{
gl_Position = gl_ModelViewProjectionMatrix * vec4(position, 1.0);
vColor = aColor;
}
''',GL_VERTEX_SHADER),
compileShader('''
#version 120
varying vec4 vColor;
uniform sampler2D uSampler;
void main()
{
gl_FragColor = vColor;
}
''',GL_FRAGMENT_SHADER),
)
glClearColor(0.0, 0.0, 0.0, 0.0) # This Will Clear The Background Color To Black
glClearDepth(1.0) # Enables Clearing Of The Depth Buffer
glEnable(GL_DEPTH_TEST)
glDepthFunc(GL_LEQUAL)
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
glShadeModel(GL_SMOOTH) # Enables Smooth Color Shading
program.position = glGetAttribLocation(program, "position")
program.color = glGetAttribLocation(program, "aColor")
initBuffers()
ReSizeGLScene(1024, 768)
def crateSquareXZ(vData, offsetX, offsetY, offsetZ, color, sizeX, sizeZ):
square = [# coords, color
offsetX-0.0, offsetY, offsetZ-0.0, color[0],color[1],color[2],color[3],
offsetX-0.0, offsetY, offsetZ+sizeZ, color[0],color[1],color[2],color[3],
offsetX+sizeX, offsetY, offsetZ+sizeZ, color[0],color[1],color[2],color[3],
offsetX+sizeX, offsetY, offsetZ+sizeZ, color[0],color[1],color[2],color[3],
offsetX+sizeX, offsetY, offsetZ-0.0, color[0],color[1],color[2],color[3],
offsetX-0.0, offsetY, offsetZ-0.0, color[0],color[1],color[2],color[3]
]
vData.extend(square)
def crateSquareXY(vData, offsetX, offsetY, offsetZ, color, sizeX, sizeY):
square = [# coords, color
offsetX-0.0, offsetY-0.0, offsetZ, color[0],color[1],color[2],color[3],
offsetX-0.0, offsetY+sizeY, offsetZ, color[0],color[1],color[2],color[3],
offsetX+sizeX, offsetY+sizeY, offsetZ, color[0],color[1],color[2],color[3],
offsetX+sizeX, offsetY+sizeY, offsetZ, color[0],color[1],color[2],color[3],
offsetX+sizeX, offsetY-0.0, offsetZ, color[0],color[1],color[2],color[3],
offsetX-0.0, offsetY-0.0, offsetZ, color[0],color[1],color[2],color[3]
]
vData.extend(square)
def crateSquareZY(vData, offsetX, offsetY, offsetZ, color, sizeZ, sizeY):
square = [# coords, color
offsetX, offsetY-0.0, offsetZ-0.0, color[0],color[1],color[2],color[3],
offsetX, offsetY+sizeY, offsetZ-0.0, color[0],color[1],color[2],color[3],
offsetX, offsetY+sizeY, offsetZ+sizeZ, color[0],color[1],color[2],color[3],
offsetX, offsetY+sizeY, offsetZ+sizeZ, color[0],color[1],color[2],color[3],
offsetX, offsetY-0.0, offsetZ+sizeZ, color[0],color[1],color[2],color[3],
offsetX, offsetY-0.0, offsetZ-0.0, color[0],color[1],color[2],color[3]
]
vData.extend(square)
def initBuffers():
vData = [# coords, color
]
# Creates the walls
crateSquareXY(vData, -50, 0, -50, [0.788,0.788,0.788,1.0], 100, 20)
crateSquareXY(vData, -50, 0, 50, [0.788,0.788,0.788,1.0], 100, 20)
crateSquareZY(vData, -50, 0, -50, [0.788,0.788,0.788,1.0], 100, 20)
crateSquareZY(vData, 50, 0, -50, [0.788,0.788,0.788,1.0], 100, 20)
# Creates the posts
crateSquareXY(vData, 20, 0, -20, [1.0,0.0,0.0,1.0], 5, 20)
crateSquareZY(vData, 20, 0, -25, [0.0,0.0,1.0,1.0], 5, 20)
crateSquareXY(vData, -25, 0, -20, [1.0,0.0,0.0,1.0], 5, 20)
crateSquareZY(vData, -20, 0, -25, [0.0,0.0,1.0,1.0], 5, 20)
crateSquareXY(vData, -25, 0, 20, [1.0,0.0,0.0,1.0], 5, 20)
crateSquareZY(vData, -20, 0, 20, [0.0,0.0,1.0,1.0], 5, 20)
crateSquareXY(vData, 20, 0, 20, [1.0,0.0,0.0,1.0], 5, 20)
crateSquareZY(vData, 20, 0, 20, [0.0,0.0,1.0,1.0], 5, 20)
# Creates the floor
colorCode = False
for i in range(-50, 50, 5):
colorCode = not colorCode
for j in range(-50, 50, 5):
colorCode = not colorCode
if colorCode == False:
color = [1.0,1.0,1.0,1.0]
else:
color = [0.0,0.0,0.0,1.0]
crateSquareXZ(vData, i, 0, j, color, 5,5)
# Creates the celing
crateSquareXZ(vData, -50, 20, -50, [0.215,0.705,1.0,1.0], 100,100)
vData = vertexBuffer(vData, 7, [3,4])
global coordBuffer
coordBuffer = vData
class vertexBuffer():
def __init__(self, coords, item_size, item_stride):
self.buffer = glGenBuffers(1)
self.itemLen = len(coords)
self.itemSize = item_size
self.itemCount = self.itemLen / self.itemSize
self.stride = item_stride
self.itemLenB = self.itemLen * 4
self.itemSizeB = self.itemSize * 4
self.strideB = map(lambda x: ctypes.c_void_p(x * 4), self.stride)
glBindBuffer(GL_ARRAY_BUFFER, self.buffer)
glBufferData(GL_ARRAY_BUFFER, numpy.array(coords, numpy.float32), GL_STATIC_DRAW)
# The function called when our window is resized (which shouldn't happen if you enable fullscreen, below)
def ReSizeGLScene(Width, Height):
if Height == 0: # Prevent A Divide By Zero If The Window Is Too Small
Height = 1
glViewport(0, 0, Width, Height) # Reset The Current Viewport And Perspective Transformation
glMatrixMode(GL_PROJECTION)
glLoadIdentity()
gluPerspective(45.0, float(Width)/float(Height), 0.1, 5000.0)
glMatrixMode(GL_MODELVIEW)
def pointerToMatrixR(pointer):
data = cast(pointer, POINTER(c_float))
matrix = [0]*16
matrix[0] = data[0]
matrix[1] = data[1]
matrix[2] = data[2]
matrix[3] = data[3]
matrix[4] = data[4]
matrix[5] = data[5]
matrix[6] = data[6]
matrix[7] = data[7]
matrix[8] = data[8]
matrix[9] = data[9]
matrix[10] = data[10]
matrix[11] = data[11]
matrix[12] = data[12]
matrix[13] = data[13]
matrix[14] = data[14]
matrix[15] = data[15]
return numpy.array(matrix, numpy.float32)
# The main drawing function.
def DrawGLScene():
global trackIRMatrix
if haveCamera:
# This is where we get the matrix pointer from the dll
# We then convert the pointer to the matrix
pointer = trackirGetMatrix()
matrix = pointerToMatrixR(pointer)
# problem with x,y,z data
matrix[12] = 0 #matrix[12] / 100
matrix[13] = 0 #matrix[13] / 100
matrix[14] = 0 #matrix[14] / 100
# If theres a identity matrix, ignore it so the display dosent go back to the center
if matrix[0] == 1 and matrix[1] == 0 and matrix[2] == 0 and matrix[3] == 0:
pass
else:
trackIRMatrix = matrix
# Clear The Screen And The Depth Buffer
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
glLoadIdentity() # Reset The View
glUseProgram(program)
glPushMatrix()
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
glMultMatrixf(trackIRMatrix)
glTranslatef(0,-5,0)
#glTranslatef(z,y,z)
glEnableVertexAttribArray(program.position)
glEnableVertexAttribArray(program.color)
glBindBuffer(GL_ARRAY_BUFFER, coordBuffer.buffer)
glVertexAttribPointer(program.position, coordBuffer.stride[0], GL_FLOAT, False, coordBuffer.itemSizeB, None)
glVertexAttribPointer(program.color, coordBuffer.stride[1], GL_FLOAT, False, coordBuffer.itemSizeB, coordBuffer.strideB[0])
glDrawArrays(GL_TRIANGLES, 0, coordBuffer.itemCount)
glDisableVertexAttribArray(program.color)
glDisableVertexAttribArray(program.position)
glPopMatrix()
glUseProgram(0)
glutSwapBuffers()
def keyPressed(*args):
if args[0] == '\x1b':
closeProgram()
elif args[0] == 'c':
trackirRecenter()
def closeProgram():
trackirStopCamera()
trackirShutdownCamera()
sys.exit()
def main():
global window
fullscreen = False
glutInit(sys.argv)
glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE | GLUT_DEPTH)
glutInitWindowSize(1024, 768)
glutInitWindowPosition(0, 0)
window = glutCreateWindow("TrackIR In Python")
# Callback functions
glutDisplayFunc(DrawGLScene)
glutIdleFunc(DrawGLScene)
glutReshapeFunc(ReSizeGLScene)
glutKeyboardFunc(keyPressed)
InitGL(1024,768)
glUseProgram(program)
glutMainLoop()
if __name__ == "__main__":
trackirSetupCamera()
if trackirAreCamerasInitialized():
haveCamera = trackirStartCamera()
main()
[/spoiler]