Skip to content

Instantly share code, notes, and snippets.

@erichoco

erichoco/ConvertCoord.cpp

Last active August 29, 2015 14:04
Show Gist options
  • Save erichoco/9fe32e3d45b55833608f to your computer and use it in GitHub Desktop.
Save erichoco/9fe32e3d45b55833608f to your computer and use it in GitHub Desktop.
Download ZIP
Conversion of 3D coordinates in Vicon System to 2D coordinates. EyeWrist.
Raw
ConvertCoord.cpp
#include <stdlib.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <deque>
#include <math.h>
using namespace std;
class Vector {
public:
double x;
double y;
double z;
Vector() {
x = y = z = 0.0;
}
Vector(double v[]) {
x = v[0];
y = v[1];
z = v[2];
}
Vector(double cx, double cy, double cz) {
x = cx;
y = cy;
z = cz;
}
Vector(deque<double> v) {
x = v.at(0);
y = v.at(1);
z = v.at(2);
}
double Norm() const {
return sqrt(x*x + y*y + z*z);
}
Vector Normalize() const {
return Vector(x/Norm(), y/Norm(), z/Norm());
}
Vector CrossProduct(const Vector &v) const
{
double nx = y * v.z - z * v.y;
double ny = z * v.x - x * v.z;
double nz = x * v.y - y * v.x;
return Vector(nx, ny, nz);
}
double DotProduct(const Vector &v) const
{
return x * v.x + y * v.y + z * v.z;
}
Vector operator + (const Vector &v) const {
return Vector(x + v.x, y + v.y, z + v.z);
}
Vector operator - (const Vector &v) const {
return Vector(x - v.x, y - v.y, z - v.z);
}
Vector operator * (double s) {
return Vector(s*x, s*y, s*z);
}
};
class Point {
public:
double x;
double y;
double z;
Point() {
x = y = z = 0.0;
}
Point(double p[]) {
x = p[0];
y = p[1];
z = p[2];
}
Point(double cx, double cy, double cz) {
x = cx;
y = cy;
z = cz;
}
Point(deque<double> p) {
x = p.at(0);
y = p.at(1);
z = p.at(2);
}
Vector operator - (const Point &p) const {
return Vector(x-p.x, y-p.y, z-p.z);
}
Point operator + (const Vector &v) const {
return Point(x + v.x, y + v.y, z + v.z);
}
Point operator - (const Vector &v) const {
return Point(x - v.x, y - v.y, z - v.z);
}
};
class ConvertCoord {
public:
Point calibO;
Point calibX;
Point calibY;
Vector normalV;
ConvertCoord() {
calibO = Point();
calibX = Point();
calibY = Point();
normalV = Vector();
cerr << "no calibration values assigned..." << endl;
}
ConvertCoord(double O[], double X[], double Y[]) {
calibO = Point(O[0], O[1], O[2]);
calibX = Point(X[0], X[1], X[2]);
calibY = Point(Y[0], Y[1], Y[2]);
tuneCalibY();
normalV = getVectorY().CrossProduct(getVectorX());
}
ConvertCoord(deque< deque<double> > c) {
calibO = Point(c.at(0));
calibX = Point(c.at(1));
calibY = Point(c.at(2));
tuneCalibY();
normalV = getVectorY().CrossProduct(getVectorX());
}
Vector getVectorX() {
return calibX - calibO;
}
Vector getVectorY() {
return calibY - calibO;
}
double get2DX(const Point& coord) {
Point coordProj = projectCoord(coord);
double x = (coordProj - calibY).CrossProduct(coordProj - calibO).Norm()
/ (getVectorY().Norm());
if (getVectorX().DotProduct(coordProj - calibO) < 0) {
x *= -1.0;
}
// x = getVectorX().Norm() - x;
return x;
}
double get2DY(const Point& coord) {
Point coordProj = projectCoord(coord);
double y = (coordProj - calibX).CrossProduct(coordProj - calibO).Norm()
/ (getVectorX().Norm());
if (getVectorY().DotProduct(coordProj - calibO) < 0) {
y *= -1.0;
}
y = getVectorY().Norm() - y;
return y;
}
double getDist(const Point& coord) {
Vector v = coord - calibO;
return v.DotProduct(normalV.Normalize());
}
Point projectCoord(const Point& coord) {
Vector unitNormalV = normalV.Normalize();
Vector v = coord - calibO;
Point coordProj = coord - unitNormalV * v.DotProduct(unitNormalV);
return coordProj;
}
deque<Point> getVertice() {
deque<Point> vertice;
return vertice;
}
private:
// 3 calibration points drawn on hand may not form an angle of 90 degree
// thus tune the position of Y
void tuneCalibY() {
Vector vectorX = getVectorX();
Vector vectorY = getVectorY();
Vector vectorX_e = vectorX.Normalize();
Vector tmp1 = vectorX_e * vectorX_e.DotProduct(vectorY);
calibY = calibO + (vectorY - tmp1);
}
};
// ref: http://stackoverflow.com/questions/1120140/how-can-i-read-and-parse-csv-files-in-c
deque<string> getNextLineAndSplitIntoTokens(istream& str) {
deque<string> result;
string line;
if (getline(str, line)) {
stringstream lineStream(line);
string cell;
while (getline(lineStream, cell, ',')) {
result.push_back(cell);
}
}
return result;
}
deque< deque<double> > getNextLineViconCoord(istream& viconCSVData, size_t coordSize) {
deque< deque<double> > coordSet;
deque<string> rawData = getNextLineAndSplitIntoTokens(viconCSVData);
if (rawData.empty() || rawData.size() < 2) {
return coordSet;
}
// coord data starts from 2nd place
rawData.pop_front();
rawData.pop_front();
for (size_t i = 0; i < coordSize; i++) {
deque<double> coord;
for (size_t j = 0; j < 3; j++) {
string val;
try {
val = rawData.at(i*3 + j);
}
catch (const out_of_range& oor) {
// cerr << "Vicon CSV data out of range: " << oor.what() << '\n';
cerr << "Vicon data lost: truncate frame" << endl;
coordSet.clear();
return coordSet;
}
coord.push_back(atof(val.c_str()));
}
coordSet.push_back(coord);
}
return coordSet;
}
string int2str(int &i) {
string s;
stringstream ss(s);
ss << i;
return ss.str();
}
void writeFile(ofstream& out, deque<double> coordX, deque<double> coordY) {
if (out.is_open()) {
out << "x,y" << endl;
for (size_t i = 0, len = coordX.size(); i < len; i++) {
out << coordX.at(i) << "," << coordY.at(i) << endl;
}
}
}
void alignAndWriteFile(string outfileName, int symbolCnt, deque< deque<double> > coordSetX, deque< deque<double> > coordSetY) {
size_t trialSize = coordSetX.size();
double maxX[trialSize];
double minX[trialSize];
double maxY[trialSize];
double minY[trialSize];
double maxXDist = 0, maxYDist = 0;
for (size_t i = 0; i < trialSize; i++) {
maxX[i] = maxY[i] = -100.0;
minX[i] = 100;
minY[i] = 100;
for (size_t j = 0, len = coordSetX.at(i).size(); j < len; j++) {
double curX = coordSetX.at(i).at(j);
double curY = coordSetY.at(i).at(j);
if (curX > maxX[i] && (curX - maxX[i]) < 200) {
maxX[i] = curX;
}
if (curX < minX[i] && (minX[i] - curX) < 200) {
minX[i] = curX;
}
if (curY > maxY[i] && (curY - maxY[i]) < 200) {
maxY[i] = curY;
}
if (curY < minY[i] && (minY[i] - curY) < 200) {
minY[i] = curY;
}
}
if ((maxX[i] - minX[i]) > maxXDist) {
maxXDist = maxX[i] - minX[i];
}
if ((maxY[i] - minY[i]) > maxYDist) {
maxYDist = maxY[i] - minY[i];
}
}
double alignX = 0; // maxXDist / 2;
double alignY = 0; // maxYDist / 2;
ofstream out;
for (int i = 0; i < trialSize; i++) {
out.open(outfileName+'-'+int2str(symbolCnt)+'-'+int2str(i)+"-a.csv");
double normalX = (maxX[i] + minX[i]) / 2;
double normalY = (maxY[i] + minY[i]) / 2;
out << "x,y" << endl;
for (int j = 0, len = coordSetX.at(i).size(); j < len; j++) {
double curX = coordSetX.at(i).at(j) - normalX + alignX;
double curY = coordSetY.at(i).at(j) - normalY + alignY;
if (out.is_open()) {
out << curX << "," << curY << endl;
}
}
out.close();
}
}
void parseViconCSV(string csvfileName, string outfileName, int markerOrder[], size_t markerSize, bool align) {
ifstream csvfile(csvfileName);
// size_t markerSize = sizeof(markerOrder)/sizeof(markerOrder[0]);
bool touch = false;
int symbolCnt = 0;
int trialCnt = 0;
const int trialSize = 1;
// ofstream outfile(outfileName+'-'+int2str(symbolCnt)+'-'+int2str(trialCnt)+".csv");
ofstream outfile;
deque< deque<double> > coordSetX;
deque< deque<double> > coordSetY;
deque<double> coordX;
deque<double> coordY;
int headerCount = 5;
while (csvfile.is_open()) {
// cout << headerCount << endl;
deque< deque<double> > coordSet = getNextLineViconCoord(csvfile, markerSize);
if (0 != headerCount) {
headerCount--;
continue;
}
if (coordSet.empty()) {
break;
}
Point index = coordSet.at(markerOrder[0]);
deque< deque<double> > calibPts;
for (size_t i = 1; i < markerSize; i++) {
calibPts.push_back(coordSet.at(markerOrder[i]));
}
ConvertCoord convert = ConvertCoord(calibPts);
double dist = convert.getDist(index);
cout << dist << endl;
if (-10 < dist && dist < 30) {
double new2DX = convert.get2DX(index);
double new2DY = convert.get2DY(index);
if (new2DX > 100 || new2DY > 100 || new2DX < -10 || new2DY < -10) {
continue;
}
else {
touch = true;
coordX.push_back(new2DX);
coordY.push_back(new2DY);
}
}
// uncomment when multi-stroke
else if (-10 < dist && dist < 100) {
}
else {
if (touch) {
// cout << endl;
touch = false;
if (!align) {
if (20 > coordX.size()) {
continue;
}
outfile.open(outfileName+int2str(symbolCnt)+'-'+int2str(trialCnt)+".csv");
// outfile.open(outfileName+"2-"+int2str(symbolCnt)+".csv");
writeFile(outfile, coordX, coordY);
outfile.close();
}
else {
coordSetX.push_back(coordX);
coordSetY.push_back(coordY);
if (trialSize-1 == trialCnt%trialSize) {
alignAndWriteFile(outfileName, symbolCnt, coordSetX, coordSetY);
coordSetX.clear();
coordSetY.clear();
}
}
coordX.clear();
coordY.clear();
if (trialSize-1 == (trialCnt++)%trialSize) {
symbolCnt++;
trialCnt = 0;
}
}
}
}
csvfile.close();
}
int main(int argc, char const *argv[]) {
// e.g. markerOrder[0] == column of index in CSV
// markerOrder[2] == column of L2 in CSV
// int markerOrder[] = {0, 2, 3, 1};
// int markerOrder[] = {3, 0, 1, 2};
int markerOrder[] = {0, 1, 2, 3};
// string csvfileName = "test-data/User0-graffiti.CSV";
// string outfileName = "output/graffiti";
string csvfileName = "test-data/tempxd-1.CSV";
string outfilePath = "../output/0813test/";
// string csvfileName = "test-data/User2-multi_stroke.CSV";
// string outfileName = "output/multi";
bool align = false;
for (int i = 1; i < argc; i++) {
if ("-a" == string(argv[i])) {
cout << "Alignment flag set..." << endl;
align = true;
}
else {
if ((i+2) == argc) {
csvfileName = string(argv[i]);
outfilePath = string(argv[i+1]);
break;
}
else {
cerr << "Output path not specified..." << endl;
exit(0);
}
}
}
string mkdir = "mkdir -p " + outfilePath;
system(mkdir.c_str());
parseViconCSV(csvfileName, outfilePath, markerOrder,
sizeof(markerOrder)/sizeof(markerOrder[0]), align);
/* Old sample code
// 3 calibration coordinates:
// O - thumb; X - index; Y - right-bottom corner
double coordO[] = {1.0, 2.0, 3.0};
double coordX[] = {2.0, 3.0, 4.0};
double coordY[] = {1.0, 3.0, 5.0};
ConvertCoord convert = ConvertCoord(coordO, coordX, coordY);
double newCoord[] = {3.0, 6.0, 0.0};
Point newPoint = Point(newCoord);
// convert new point to 2D coordinate
double new2DX = convert.get2DX(newPoint);
double new2DY = convert.get2DY(newPoint);
cout << new2DX << endl;
cout << new2DY << endl;
*/
return 0;
}
@erjan
Copy link

erjan commented Jul 19, 2014

nice job

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment