Skip to content

M
MasterArbeit
Commit 9f572c13 by wester
Options

Update

parent 63e925bc
Showing with 81 additions and 39 deletions
3_PointCloud/main.cpp
......@@ -45,9 +45,9 @@ IMultiSourceFrameReader* reader; // Kinect data source
ICoordinateMapper* mapper; // Converts between depth, color, and 3d coordinates
TIMESPAN colorFrameTime = -1;
float KinectSizeX = 0.249;
float KinectSizeY = 0.042; // 42 mm, 66mm ist die gesammthhe
float KinectSizeZ = 0.067;
float KinectSizeX = 0.249f;
float KinectSizeY = 0.042f; // 42 mm, 66mm ist die gesammthhe
float KinectSizeZ = 0.067f;
float KinectOriginOffsetX = ((92 + 73) / 2.0f) / 1000.0f;
float KinectOriginOffsetY = KinectSizeY/2.0f;
......@@ -60,7 +60,11 @@ glm::vec3 FrameTranslation(0.0,0.0,0.0);
SteamTracking* steamTracking = nullptr;
char* controllerSerial = "LHR-FFFDBD41";
int controllerID = 1;
char* trackerSerial = "LHR-18DC0D92";
int trackerID = 5;
glm::mat4x4* currentControlerPos = nullptr;
glm::mat4x4* currentTrackerPos = nullptr;
bool useTrackerTransform = true;
glm::vec3 lastClicketControlelrPos;
glm::mat4x4 controlerToKinect;
std::vector<glm::vec3> controllerPositions;
......@@ -80,10 +84,10 @@ int lastX, lastY;
//Filter Settings
float minTrackingKinect = 0.2f;//0.8f;
float maxTrackingKinect = 1.8f;// 2.5f;
float maxTrackingKinect = 1.0f;// 2.5f;
//Custom
//float angleDegrees = 20.0f;
float maxNormalAngle = degreeToRadioan(25.0f);
float maxNormalAngle = degreeToRadian(25.0f);
bool enableDepthFilter = false;
bool enableNormalFilter = false;//true;
//Bilateral / FastDepthNoise
......@@ -94,8 +98,8 @@ float repairThreshold = 100.0f;
float degreeToRadioan(float degree) {
return degree*M_PI / 180.0;
float degreeToRadian(float degree) {
return (float) (degree*M_PI / 180.0f);
}
......@@ -231,8 +235,8 @@ void getDepthData(IMultiSourceFrame* frame) {
if ((enableNormalFilter || enableDepthFilter) && x > 0 && y > 0 && x < width - 1 && y < height - 1) {
if (enableNormalFilter) {
//normalen
float dzdx = (curr[x + 1 + width * y] - curr[x - 1 + width * y]) / 2.0;
float dzdy = (curr[x + width * (y + 1)] - curr[x + width * (y - 1)]) / 2.0;
float dzdx = (curr[x + 1 + width * y] - curr[x - 1 + width * y]) / 2.0f;
float dzdy = (curr[x + width * (y + 1)] - curr[x + width * (y - 1)]) / 2.0f;
glm::vec3 d(-dzdx, -dzdy, 1.0f);
frameNormals[i] = normalize(d);
......@@ -385,10 +389,13 @@ void processCurrentFrameForExport() {
float maxx, maxy, maxz;
maxx = maxy = maxz = -std::numeric_limits<float>::max();
bool captureFramesTXT = false;
std::ofstream myfile;
char buffer[64]; // make sure it's big enough
snprintf(buffer, sizeof(buffer), "frames/pnts_%d.txt", counter++);
myfile.open(buffer, std::ios::out | std::ios::binary);
if (captureFramesTXT) {
char buffer[64]; // make sure it's big enough
snprintf(buffer, sizeof(buffer), "frames/pnts_%d.txt", counter++);
myfile.open(buffer, std::ios::out | std::ios::binary);
}
for (unsigned int i = 0; i < width*height; i++) {
if (frameValidPoints[i]) {
......@@ -411,17 +418,17 @@ void processCurrentFrameForExport() {
colors.push_back(frameColors[i * 3]);
colors.push_back(frameColors[i * 3 + 1]);
colors.push_back(frameColors[i * 3 + 2]);
myfile << positionGlobal.x << " "<<positionGlobal.y << " " << positionGlobal.z << " "
<< (int)frameColors[i * 3] << " " << (int)frameColors[i * 3 + 1] << " " << (int)frameColors[i * 3 + 2] << std::endl;
if (captureFramesTXT) {
myfile << positionGlobal.x << " " << positionGlobal.y << " " << positionGlobal.z << " "
<< (int)frameColors[i * 3] << " " << (int)frameColors[i * 3 + 1] << " " << (int)frameColors[i * 3 + 2] << std::endl;
}
}
}
myfile.close();
if (captureFramesTXT) {
myfile.close();
}
std::cout << "Frame Local Boundingbox Min: [" << minx << "|" << miny << "|" << minz << "] Max: [" << maxx << "|" << maxy << "|" << maxz << "]"<< std::endl;
//update Buffers
if (points.size() == 0) return;
......@@ -444,25 +451,47 @@ void writePointCloud() {
return;
}
if (!currentTrackerPos)
currentTrackerPos = new glm::mat4x4(glm::rotate(*steamTracking->getTransformationForDevice(trackerID, 0.0f), degreeToRadian(-00), glm::vec3(1.0f, 0.0f, 0.0f)));
std::vector<glm::vec3> exportpoints;
exportpoints.reserve(points.size());
for (size_t i = 0; i < points.size(); i++) {
exportpoints.push_back(glm::vec3(glm::rotate(glm::inverse(*currentTrackerPos), degreeToRadian(00), glm::vec3(1.0f, 0.0f, 0.0f)) * glm::vec4(points[i], 1.0f)));
}
//exportpoints.clear();
for (int i = 0; i < 1000; i++) {
exportpoints.push_back(glm::vec3(i/1000.0f, 0.0f, 0.0f));
exportpoints.push_back(glm::vec3(0.0f, i / 1000.0f, 0.0f));
exportpoints.push_back(glm::vec3(0.0f, i / 2000.0f, 1.0f));
exportpoints.push_back(glm::vec3(0.0f, 0.0f, i / 1000.0f));
}
pntsHeader header;
size_t pointArrayByteSize = points.size() * sizeof(glm::vec3);
std::cout << points.size() << " " << sizeof(glm::vec3);
size_t pointArrayByteSize = exportpoints.size() * sizeof(glm::vec3);
std::cout<< "writing File: Number of Points:" << exportpoints.size() ;
size_t coloArrayByteSize = colors.size() * sizeof(unsigned char);
header.featureTableByteLenght = pointArrayByteSize + coloArrayByteSize;
std::ostringstream os;
os << "{\"POINTS_LENGTH\":" << (points.size() ) << ", \"POSITION\" : {\"byteOffset\":0}, \"RGB\" : {\"byteOffset\":" << pointArrayByteSize << "}}";
os << "{\"POINTS_LENGTH\":" << (exportpoints.size() ) << ", \"POSITION\" : {\"byteOffset\":0}, \"RGB\" : {\"byteOffset\":" << pointArrayByteSize << "}}";
std::string json = os.str();
header.featureTableJSONByteLenght = json.size();
header.byteLenght = 28 /*header*/ + header.featureTableJSONByteLenght + header.featureTableByteLenght;
std::cout << "File Size " << header.byteLenght;
std::cout << " File Size in byte" << header.byteLenght <<std::endl;
std::ofstream myfile;
myfile.open("example.pnts", std::ios::out | std::ios::binary);
myfile.write((char*)&header, 28);
myfile << json;
myfile.write(reinterpret_cast<const char*>(&points[0]), pointArrayByteSize);
myfile.write(reinterpret_cast<const char*>(&exportpoints[0]), pointArrayByteSize);
myfile.write(reinterpret_cast<const char*>(&colors[0]), coloArrayByteSize);
//myfile <<
......@@ -507,6 +536,11 @@ void mainRenderLoop() {
//get controler pos
delete currentControlerPos;
currentControlerPos = steamTracking->getTransformationForDevice(controllerID, 0.0f);
delete currentTrackerPos;
currentTrackerPos = steamTracking->getTransformationForDevice(trackerID, 0.0f);
currentTrackerPos = new glm::mat4x4(glm::rotate(*currentTrackerPos, degreeToRadian(-90), glm::vec3(1.0f, 0.0f, 0.0f)));
if (trackControllerPos) {
glm::vec3 tempPos = glm::vec3(*currentControlerPos*glm::vec4(0, 0, 0, 1));
......@@ -528,8 +562,11 @@ void mainRenderLoop() {
//add points to point cloud
if (captureFrame) {
processCurrentFrameForExport();
glm::vec3 TrackerPos = glm::vec3(*steamTracking->getTransformationForDevice(trackerID, 0.0f)*glm::vec4(0, 0, 0, 1));
glm::vec3 current = controllerPositions.back();
std::cout << "Current Position [" << current.x << "/" << current.y << "/" << current.z << "] distance " << distance(lastClicketControlelrPos, current) << std::endl ;
std::cout << "Current Position [" << current.x << "/" << current.y << "/" << current.z << "] TrackerPos: [" << TrackerPos.x << "/" << TrackerPos.y << "/" << TrackerPos.z << "] distance " << distance(lastClicketControlelrPos, current) << std::endl ;
lastClicketControlelrPos = controllerPositions.back();
FrameTranslation = glm::vec3(0.0, 0.0, 0.0);
......@@ -546,7 +583,7 @@ void mainRenderLoop() {
//rotateCamera(); //todo
if (toggleCam && trackControllerPos) {
glTranslatef(0.0, -0.3, -1.0);
glTranslatef(0.0f, -0.3f, -1.0f);
glRotatef(10.0, 1.0, 0.0, 0.0);
glRotatef(180.0, 0.0, 1.0, 0.0);
glMultMatrixf(glm::value_ptr(glm::inverse(controlerToKinect)));
......@@ -582,10 +619,15 @@ void mainRenderLoop() {
glVertexPointer(3, GL_FLOAT, 0, NULL);
glDrawArrays(GL_LINE_STRIP, 0, controllerPosData.size);
}
glPushMatrix();
glMultMatrixf(glm::value_ptr(*currentTrackerPos));
DrawCoordinateSystem();
glPopMatrix();
{ // Controller Space
glPushMatrix();
if (currentControlerPos) { //applay controler Transformation
glMultMatrixf(glm::value_ptr(*currentControlerPos));
}
......@@ -629,7 +671,7 @@ void mainRenderLoop() {
}
void moveCurrentFrame(glm::vec3 dir) {
float factor = 0.002;
float factor = 0.002f;
FrameTranslation += factor*dir;
std::cout << "Frame Transformation ";
printVector(FrameTranslation);
......@@ -646,7 +688,6 @@ void keyPressed(unsigned char key, int x, int y)
captureFrame = true;
break;
case 'x':
std::cout << "writing File" << std::endl;
writeFile = true;
break;
case 'c':
......@@ -727,6 +768,7 @@ int main(int argc, char* argv[]) {
steamTracking->printVRDevices();
steamTracking->onButtonPress = captureNextFrame;
controllerID = steamTracking->getDeviceIDForSerial(controllerSerial);
trackerID = steamTracking->getDeviceIDForSerial(trackerSerial);
// OpenGL setup
......@@ -771,13 +813,13 @@ int main(int argc, char* argv[]) {
//controlerToKinect = glm::translate(glm::mat4x4(), (glm::vec3(0.0f, 0.0f,0.173 /*controller*/+0.03 /*Halterung*/ )));
controlerToKinect = glm::mat4x4();
controlerToKinect = glm::rotate(controlerToKinect, degreeToRadioan(-58+180 /*63,9...*/), glm::vec3(1.0f, 0.0f, 0.0f));
controlerToKinect = glm::rotate(controlerToKinect, degreeToRadioan(180.0f), glm::vec3(0.0f, 1.0f, 0.0f));
controlerToKinect = glm::rotate(controlerToKinect, degreeToRadian(-58+180 /*63,9...*/), glm::vec3(1.0f, 0.0f, 0.0f));
controlerToKinect = glm::rotate(controlerToKinect, degreeToRadian(180.0f), glm::vec3(0.0f, 1.0f, 0.0f));
//controlerToKinect = glm::rotate(controlerToKinect, degreeToRadioan(180.0f), glm::vec3(0.0f, 0.0f, 1.0f));
float dx = 0.010;// 0.00;
float dy = KinectSizeY / 2.0f + 0.004 /*HAlterung Platte*/ + 0.011 /* Conroller*/;
float dz = -0.01; //-0.023 + 0.02; // 0.045f /*= versatz auf der Halterung; Distanz zur Halterung:*/ + (KinectSizeZ - (0.066 /*halterung z*/ - 0.012 /*halterung berstand*/));
float dx = 0.000f;// 0.01;
float dy = KinectSizeY / 2.0f + 0.004f /*HAlterung Platte*/ + 0.011f /* Conroller*/;
float dz = -0.01f; //-0.023 + 0.02; // 0.045f /*= versatz auf der Halterung; Distanz zur Halterung:*/ + (KinectSizeZ - (0.066 /*halterung z*/ - 0.012 /*halterung berstand*/));
controlerToKinect = glm::translate(controlerToKinect, (glm::vec3(dx, -dy, dz)));
......@@ -792,17 +834,17 @@ int main(int argc, char* argv[]) {
glm::mat4 kinectToController = glm::inverse(controlerToKinect);
float angleInDegrees = 30;
glm::vec4 test = controlerToKinect * glm::rotate(glm::mat4(), degreeToRadioan(angleInDegrees), glm::vec3(1.0f, 0.0f, 0.0f)) * kinectToController * glm::vec4(0.0, 0.0, 1.0, 1.0);
glm::vec4 test = controlerToKinect * glm::rotate(glm::mat4(), degreeToRadian(angleInDegrees), glm::vec3(1.0f, 0.0f, 0.0f)) * kinectToController * glm::vec4(0.0, 0.0, 1.0, 1.0);
std::cout << "Angle Error x ";
printVector(test);
std::cout << std::endl;
test = controlerToKinect * glm::rotate(glm::mat4(), degreeToRadioan(angleInDegrees), glm::vec3(0.0f, 1.0f, 0.0f)) * kinectToController * glm::vec4(0.0, 0.0, 1.0, 1.0);
test = controlerToKinect * glm::rotate(glm::mat4(), degreeToRadian(angleInDegrees), glm::vec3(0.0f, 1.0f, 0.0f)) * kinectToController * glm::vec4(0.0, 0.0, 1.0, 1.0);
std::cout << "Angle Error y ";
printVector(test);
std::cout << std::endl;
test = controlerToKinect * glm::rotate(glm::mat4(), degreeToRadioan(angleInDegrees), glm::vec3(0.0f, 0.0f, 1.0f)) * kinectToController * glm::vec4(0.0, 0.0, 1.0, 1.0);
test = controlerToKinect * glm::rotate(glm::mat4(), degreeToRadian(angleInDegrees), glm::vec3(0.0f, 0.0f, 1.0f)) * kinectToController * glm::vec4(0.0, 0.0, 1.0, 1.0);
std::cout << "Angle Error z ";
printVector(test);
std::cout << std::endl;
......
3_PointCloud/main.h
......@@ -45,7 +45,7 @@ void setupFrameBuffer();
void reshapeFunction(int width, int height);
void printVector(glm::vec3 vector);
float degreeToRadioan(float degree);
float degreeToRadian(float degree);
struct pntsHeader {
......
3_PointCloud/tileset.json → 3_PointCloud/tileset1.json
......@@ -18,7 +18,7 @@
{
"transform": [
1.0, 0.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0,
0.0, 1.0, 0.0, 50.0,
0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 1.0
],
......
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