Permalink
Browse files

Still working on the eye calibration. Currently I'm running some test…

…'s that might cause the program to crash if ran on another computer.
  • Loading branch information...
1 parent ca2f6aa commit 8505d2f0df58ba7d2309362a8ff26f848210d811 @mdfeist committed Jun 8, 2013
View
98 BlinkAnalysis/ClientHandler.cpp
@@ -7,6 +7,12 @@
#include <string.h>
+#include <iostream>
+#include <fstream>
+#include <windows.h>
+
+using namespace std;
+
// Dikablis
int ClientHandler::DikablisViewingWidth = 768;
int ClientHandler::DikablisViewingHeight = 576;
@@ -50,6 +56,98 @@ ClientHandler::~ClientHandler(void)
free(this->dikablisEyeVectorArray);
}
+void ClientHandler::setRay(osg::Vec3 ray, int x, int y) {
+ if (this->dikablisEyeVectorArray != NULL) {
+ int i = x + DikablisViewingMargin;
+ int j = y + DikablisViewingMargin;
+
+ int loc = (j*DikablisViewingWidth*2*DikablisViewingMargin) + i;
+ loc *= 3;
+ if (loc >= 0 && loc < DikablisViewingSize - 3) {
+ this->dikablisEyeVectorArray[loc + 0] = ray.x();
+ this->dikablisEyeVectorArray[loc + 1] = ray.y();
+ this->dikablisEyeVectorArray[loc + 2] = ray.z();
+ }
+ }
+}
+
+osg::Vec3 ClientHandler::getRay(int x, int y) {
+ osg::Vec3 ray;
+
+ if (this->dikablisEyeVectorArray != NULL) {
+ int i = x + DikablisViewingMargin;
+ int j = y + DikablisViewingMargin;
+
+ int loc = (3*j*DikablisViewingWidth*2*DikablisViewingMargin) + i;
+
+ ray.x() = this->dikablisEyeVectorArray[loc + 0];
+ ray.y() = this->dikablisEyeVectorArray[loc + 1];
+ ray.z() = this->dikablisEyeVectorArray[loc + 2];
+ }
+
+ return ray;
+}
+
+void ClientHandler::save() {
+ int width = 512;
+ int height = 512;
+
+ HANDLE file;
+ BITMAPFILEHEADER fileHeader;
+ BITMAPINFOHEADER fileInfo;
+ RGBTRIPLE *image;
+ DWORD write = 0;
+ image = new RGBTRIPLE[width*height*24];
+
+ file = CreateFile(L"C:\\Users\\mdfeist\\Desktop\\rayMap.bmp",GENERIC_WRITE,0,NULL,CREATE_ALWAYS,FILE_ATTRIBUTE_NORMAL,NULL); //Sets up the new bmp to be written to
+
+ fileHeader.bfType = 19778; //Sets our type to BM or bmp
+ fileHeader.bfSize = sizeof(fileHeader.bfOffBits) + sizeof(RGBTRIPLE); //Sets the size equal to the size of the header struct
+ fileHeader.bfReserved1 = 0; //sets the reserves to 0
+ fileHeader.bfReserved2 = 0;
+ fileHeader.bfOffBits = sizeof(BITMAPFILEHEADER)+sizeof(BITMAPINFOHEADER); //Sets offbits equal to the size of file and info header
+
+ fileInfo.biSize = sizeof(BITMAPINFOHEADER);
+ fileInfo.biWidth = width;
+ fileInfo.biHeight = height;
+ fileInfo.biPlanes = 1;
+ fileInfo.biBitCount = 24;
+ fileInfo.biCompression = BI_RGB;
+ fileInfo.biSizeImage = width * height * (24/8);
+ fileInfo.biXPelsPerMeter = 2400;
+ fileInfo.biYPelsPerMeter = 2400;
+ fileInfo.biClrImportant = 0;
+ fileInfo.biClrUsed = 0;
+
+ WriteFile(file,&fileHeader,sizeof(fileHeader),&write,NULL);
+ WriteFile(file,&fileInfo,sizeof(fileInfo),&write,NULL);
+
+ for (int i = 0; i < width*height*24; i++)
+ {
+ int index = i / 24;
+
+ if (index >= 0 && index < DikablisViewingSize - 3) {
+ int b = 10*(this->dikablisEyeVectorArray[3*index + 0] + 20);
+ int g = 10*(this->dikablisEyeVectorArray[3*index + 1] + 20);
+ int r = 10*(this->dikablisEyeVectorArray[3*index + 2] + 20);
+
+
+ image[i].rgbtBlue = b;
+ image[i].rgbtGreen = g;
+ image[i].rgbtRed = r;
+ } else {
+ image[i].rgbtBlue = 255;
+ image[i].rgbtGreen = 255;
+ image[i].rgbtRed = 255;
+ }
+ }
+
+ WriteFile(file, image, fileInfo.biSizeImage, &write, NULL);
+
+ CloseHandle(file);
+}
+
+
// Add a Rigid Body to the ClientHandler
bool ClientHandler::addRigidBody(int id, RigidBody* rigidBody)
{
View
4 BlinkAnalysis/ClientHandler.h
@@ -151,5 +151,9 @@ class ClientHandler
static int getDikablisViewingHeight() { return DikablisViewingHeight; }
static int getDikablisViewingMargin() { return DikablisViewingMargin; }
static int getDikablisViewingSize() { return DikablisViewingSize; }
+
+ void setRay(osg::Vec3 ray, int x, int y);
+ osg::Vec3 getRay(int x, int y);
+ void save();
};
View
304 BlinkAnalysis/EyeCalibration.cpp
@@ -5,6 +5,9 @@
#include "EyeCalibrationWizardFormController.h"
#include <algorithm> // std::sort
+#include <iostream>
+#include <fstream>
+
#include <osg/Matrix>
EyeCalibration::EyeCalibration(void) {
@@ -365,89 +368,288 @@ bool EyeCalibration::calibrate() {
EyeCalibrationWizardFormController::getInstance()->calibrationOutputLog("\n");
}
+ // Save calibrarion
+ /*
+ bool fileOpen = false;
+ std::ofstream myfile ("C:\\Users\\mdfeist\\Desktop\\segments.txt");
+ if (myfile.is_open())
+ {
+ fileOpen = true;
+ }
+ */
+
// Calculate Inner Interpolation
- std::vector<CalibrationPoint> boundingPoints;
- std::vector<Segment> subHull;
- for (unsigned int i = 0; i < innerUnknownPoints.size(); i++) {
- CalibrationPoint point = innerUnknownPoints.at(i);
+ for (int innerPointIndex = innerUnknownPoints.size() - 1; innerPointIndex >= 0; innerPointIndex--) {
+ CalibrationPoint innerPoint = innerUnknownPoints.at(innerPointIndex);
- bool foundBoundingHull = false;
+ std::vector<CalibrationPoint> boundingPoints;
+ std::vector<Segment> boundingEdges;
- if (boundingPoints.size() >= 4 && subHull.size() != 0) {
- if (pointInPolygon(subHull, point) <= 0) {
- foundBoundingHull = true;
- }
+ std::vector<CalibrationPoint> possibleBoundingPoints(calibrationPoints);
+ std::sort (possibleBoundingPoints.begin(), possibleBoundingPoints.end(), ComparePointsDistanceFrom(this, innerPoint));
+
+ for (unsigned int boundingIndex = 0; boundingIndex < 4; boundingIndex++) {
+ boundingPoints.push_back(possibleBoundingPoints.back());
+ possibleBoundingPoints.pop_back();
}
- if (!foundBoundingHull) {
- std::vector<CalibrationPoint> possibleBoundingPoints(orderedPoints);
- std::sort (possibleBoundingPoints.begin(), possibleBoundingPoints.end(), ComparePointsDistanceFrom(this, point));
+ bool foundBoundingHull = false;
+ while (!foundBoundingHull) {
+ if (boundingPoints.size() != 4) {
+ break;
+ }
+
+ // Get min and max
+ CalibrationPoint
+ minX = boundingPoints.at(0),
+ minY = boundingPoints.at(0),
+ maxX = boundingPoints.at(0),
+ maxY = boundingPoints.at(0);
+
+ for (unsigned int boundingIndex = 0; boundingIndex < boundingPoints.size(); boundingIndex++) {
+ CalibrationPoint point = boundingPoints.at(boundingIndex);
+
+ if (minX.x() > point.x())
+ minX = point;
+
+ if (minY.y() > point.y())
+ minY = point;
- unsigned int pi = 0;
- while (pi < 4) {
- CalibrationPoint boundingPoint = possibleBoundingPoints.at(pi);
- boundingPoints.push_back(boundingPoint);
+ if (maxX.x() < point.x())
+ maxX = point;
- pi++;
+ if (maxY.y() < point.y())
+ maxY = point;
}
- while (!foundBoundingHull) {
- subHull = calculateConvexHull(boundingPoints);
- if (pointInPolygon(subHull, point) <= 0) {
- foundBoundingHull = true;
+ // Get center of bounding polygon
+ int boundingCenterX = (maxX.x() - minX.x())/2;
+ int boundingCenterY = (maxY.y() - minY.y())/2;
+
+ // Calculate Ray through center
+ int ray_x = innerPoint.x() - boundingCenterX;
+ int ray_y = innerPoint.y() - boundingCenterY;
+
+ ray_x *= 1000;
+ ray_y *= 1000;
+
+ CalibrationPoint pointRayEnd(ray_x, ray_y, osg::Vec3(0.f, 0.f, 0.f));
+ Segment ray(innerPoint, pointRayEnd);
+
+ // Get bounding edges
+ std::sort (boundingPoints.begin(), boundingPoints.end(), ComparePoints(this));
+ boundingEdges.clear();
+
+ for (unsigned int i = 0; i < boundingPoints.size(); i++) {
+ if (i < boundingPoints.size() - 1) {
+ boundingEdges.push_back( Segment(boundingPoints.at(i), boundingPoints.at(i + 1)) );
} else {
+ boundingEdges.push_back( Segment(boundingPoints.at(i), boundingPoints.at(0)) );
+ }
+ }
- for (int _times = 0; _times < 2; _times++) {
+ // Used to check and see if point is contained in bounding points
+ unsigned int collisionCount = 0;
+ for (unsigned int i = 0; i < boundingEdges.size(); i++) {
+ if ( getLineIntersection(ray, boundingEdges.at(i)) )
+ collisionCount++;
+ }
- for (int _rm = boundingPoints.size() - 1; _rm >= 0; _rm--) {
- if (pointInPolygon(convexHull, boundingPoints.at(_rm)) < 0) {
- boundingPoints.erase(boundingPoints.begin() + _rm);
- break;
- }
- }
- }
+ // Print
+ /*
+ myfile << "Point (" << innerPoint.x() << ", " << innerPoint.y() << ")";
+ if (collisionCount % 2 == 1) {
+ myfile << " Bounded By ";
+ } else {
+ myfile << " Not Bounded By ";
+ }
- if (boundingPoints.size() >= 4) {
- sprintf_s(buf, "Point %d: (%d, %d) pi %d\n",
- i + 1,
- point.x(), point.y(), pi + 1);
- EyeCalibrationWizardFormController::getInstance()->calibrationOutputLog(buf);
+ for (unsigned int i = 0; i < boundingPoints.size(); i++) {
+ CalibrationPoint point = boundingPoints.at(i);
+ myfile << " (" << point.x() << ", " << point.y() << ")";
+ }
- EyeCalibrationWizardFormController::getInstance()->calibrationOutputLog("Failed: Unable to create bounding box for point.\n");
- break;
+ myfile << "\n";
+ */
+ if (collisionCount % 2 == 1) {
+ foundBoundingHull = true;
+ } else {
+ int closestEdge = -1;
+ float closestDistance = -1.f;
+ for (unsigned int i = 0; i < boundingEdges.size(); i++) {
+ Segment edge = boundingEdges.at(i);
+ CalibrationPoint closest = getClosestPoint(edge.p1(), edge.p2(), innerPoint, true);
+
+ if (!(closest.x() == edge.x1() && closest.y() == edge.y1()) &&
+ !(closest.x() == edge.x2() && closest.y() == edge.y2())) {
+ float dif_x = closest.x()-innerPoint.x();
+ float dif_y = closest.y()-innerPoint.y();
+
+ float distance = (dif_x)*(dif_x) + (dif_y)*(dif_y);
+ if (closestDistance < 0.f || closestDistance > distance) {
+ closestDistance = distance;
+ closestEdge = i;
+ }
}
+ }
- while (pi < possibleBoundingPoints.size() && boundingPoints.size() < 4) {
- boundingPoints.push_back(possibleBoundingPoints.at(pi));
- pi++;
- }
+ if (closestEdge != -1) {
+ Segment edge = boundingEdges.at(closestEdge);
- if (boundingPoints.size() < 4) {
-
- sprintf_s(buf, "Point %d: (%d, %d) pi %d\n",
- i + 1,
- point.x(), point.y(), pi + 1);
- EyeCalibrationWizardFormController::getInstance()->calibrationOutputLog(buf);
+ for (int i = boundingPoints.size() - 1; i >= 0; i--) {
+ CalibrationPoint point = boundingPoints.at(i);
+ if (!(point.x() == edge.x1() && point.y() == edge.y1()) &&
+ !(point.x() == edge.x2() && point.y() == edge.y2())) {
+ boundingPoints.erase(boundingPoints.begin()+i);
+ }
+ }
- EyeCalibrationWizardFormController::getInstance()->calibrationOutputLog("Unable to create bounding box for point.\n");
+ if (possibleBoundingPoints.size() >= 2) {
+ boundingPoints.push_back(possibleBoundingPoints.back());
+ possibleBoundingPoints.pop_back();
+ boundingPoints.push_back(possibleBoundingPoints.back());
+ possibleBoundingPoints.pop_back();
+ } else {
break;
}
+ } else {
+ break;
}
}
}
-
-
- if (!foundBoundingHull)
- continue;
+ if (!foundBoundingHull) {
+ outerUnknownPoints.push_back(innerPoint);
+ innerUnknownPoints.erase(innerUnknownPoints.begin()+innerPointIndex);
+ } else {
+ CalibrationPoint closestPoints[4];
+ for (unsigned int i = 0; i < boundingEdges.size(); i++) {
+ Segment edge = boundingEdges.at(i);
+ closestPoints[i] = getClosestPoint(edge.p1(), edge.p2(), innerPoint, true);
+ }
+
+ CalibrationPoint
+ minX = closestPoints[0],
+ minY = closestPoints[0],
+ maxX = closestPoints[0],
+ maxY = closestPoints[0];
+
+ for (unsigned int i = 0; i < boundingEdges.size(); i++) {
+ if (minX.x() > closestPoints[i].x())
+ minX = closestPoints[i];
+
+ if (minY.y() > closestPoints[i].y())
+ minY = closestPoints[i];
+
+ if (maxX.x() < closestPoints[i].x())
+ maxX = closestPoints[i];
+
+ if (maxY.y() < closestPoints[i].y())
+ maxY = closestPoints[i];
+ }
+
+ float t;
+ t = (innerPoint.x() - minX.x()) / (maxX.x() - minX.x());
+ osg::Vec3 ray_x = minX.getRay() + (maxX.getRay() - minX.getRay()) * t;
+
+ t = (innerPoint.y() - minY.y()) / (maxY.y() - minY.y());
+ osg::Vec3 ray_y = minY.getRay() + (maxY.getRay() - minY.getRay()) * t;
+
+ osg::Vec3 ray = (ray_x + ray_y) / 2.f;
+
+ ClientHandler* client = AppData::getInstance()->getClient();
+
+ if (client) {
+ client->setRay(ray, innerPoint.x(), innerPoint.y());
+ }
+ }
+ }
+
+ //myfile.close();
+
+ ClientHandler* client = AppData::getInstance()->getClient();
+
+ if (client) {
+ client->save();
}
EyeCalibrationWizardFormController::getInstance()->calibrationOutputLog("Done\n");
return true;
}
+std::vector<EyeCalibration::Segment> EyeCalibration::getEdges(std::vector<CalibrationPoint> processingPoints) {
+ std::vector<Segment> edges;
+ for (unsigned int i = 0; i < processingPoints.size(); i++) {
+ for (unsigned int j = i + 1; j < processingPoints.size(); j++) {
+ edges.push_back( Segment(processingPoints.at(i), processingPoints.at(j)) );
+ }
+ }
+
+ return edges;
+}
+
+// Returns 1 if the lines intersect, otherwise 0.
+int EyeCalibration::getLineIntersection(Segment edge1, Segment edge2)
+{
+ int p0_x, p0_y, p1_x, p1_y, p2_x, p2_y, p3_x, p3_y;
+ // Edge 1
+ p0_x = edge1.x1();
+ p0_y = edge1.y1();
+ p1_x = edge1.x2();
+ p1_y = edge1.y2();
+
+ // Edge 2
+ p2_x = edge2.x1();
+ p2_y = edge2.y1();
+ p3_x = edge2.x2();
+ p3_y = edge2.y2();
+
+ float s1_x, s1_y, s2_x, s2_y;
+ s1_x = p1_x - p0_x; s1_y = p1_y - p0_y;
+ s2_x = p3_x - p2_x; s2_y = p3_y - p2_y;
+
+ float s, t;
+ s = (-s1_y * (p0_x - p2_x) + s1_x * (p0_y - p2_y)) / (-s2_x * s1_y + s1_x * s2_y);
+ t = ( s2_x * (p0_y - p2_y) - s2_y * (p0_x - p2_x)) / (-s2_x * s1_y + s1_x * s2_y);
+
+ if (s >= 0 && s <= 1 && t >= 0 && t <= 1)
+ {
+ // Collision detected
+ return 1;
+ }
+
+ return 0; // No collision
+}
+
+EyeCalibration::CalibrationPoint EyeCalibration::getClosestPoint(CalibrationPoint a, CalibrationPoint b, CalibrationPoint point, bool segmentClamp) {
+ int ap_x = point.x() - a.x();
+ int ap_y = point.y() - a.y();
+
+ int ab_x = b.x() - a.x();
+ int ab_y = b.y() - a.y();
+
+ float ab2 = ab_x*ab_x + ab_y*ab_y;
+ float ap_ab = ap_x*ab_x + ap_y*ab_y;
+
+ float t = ap_ab / ab2;
+
+ if (segmentClamp) {
+ if (t < 0.f) t = 0.f;
+ else if (t > 1.f) t = 1.f;
+ }
+
+ int closest_x = a.x() + ab_x * t;
+ int closest_y = a.y() + ab_y * t;
+
+ osg::Vec3 ab_ray = b.getRay() - a.getRay();
+ osg::Vec3 ray = a.getRay() + ab_ray * t;
+
+ CalibrationPoint closest(closest_x, closest_y, ray);
+ return closest;
+}
+
int EyeCalibration::pointInPolygon(std::vector<Segment> hull, CalibrationPoint point) {
unsigned int n = hull.size();
View
5 BlinkAnalysis/EyeCalibration.h
@@ -59,6 +59,9 @@ class EyeCalibration {
int isLeft(Segment segment, CalibrationPoint r);
std::vector<Segment> calculateConvexHull(std::vector<CalibrationPoint> processingPoints);
+ std::vector<Segment> getEdges(std::vector<CalibrationPoint> processingPoints);
+ int getLineIntersection(Segment edge1, Segment edge2);
+ CalibrationPoint getClosestPoint(CalibrationPoint a, CalibrationPoint b, CalibrationPoint point, bool segmentClamp);
struct ComparePoints : std::binary_function<CalibrationPoint, CalibrationPoint, bool> {
ComparePoints(EyeCalibration * cal) : _cal(cal) {}
@@ -75,7 +78,7 @@ class EyeCalibration {
float distanceA = (_point.x()-a.x())*(_point.x()-a.x()) + (_point.y()-a.y())*(_point.y()-a.y());
float distanceB = (_point.x()-b.x())*(_point.x()-b.x()) + (_point.y()-b.y())*(_point.y()-b.y());
- if (distanceA < distanceB)
+ if (distanceA > distanceB)
return true;
return false;
View
BIN BlinkAnalysis/慲䵹灡戮灭
Binary file not shown.

0 comments on commit 8505d2f

Please sign in to comment.