Commit 45a0ff97 authored by Kaspar Scharf's avatar Kaspar Scharf

BACKUP

parent 6a5a3f87
......@@ -37,7 +37,7 @@
<ConfirmationsSetting value="0" id="Add" />
<ConfirmationsSetting value="0" id="Remove" />
</component>
<component name="ProjectRootManager" version="2" languageLevel="JDK_1_8" default="true" assert-keyword="true" jdk-15="true" project-jdk-name="1.8" project-jdk-type="JavaSDK">
<component name="ProjectRootManager" version="2" languageLevel="JDK_1_7" default="true" assert-keyword="true" jdk-15="true" project-jdk-name="1.8" project-jdk-type="JavaSDK">
<output url="file://$PROJECT_DIR$/build/classes" />
</component>
<component name="ProjectType">
......
......@@ -14,7 +14,7 @@
std::string PostProcessing::R2S(float f) {
std::stringstream stringStream;
stringStream << f;
stringStream << std::setprecision(std::numeric_limits<double>::digits10 + 1) << f;
return stringStream.str();
}
......@@ -24,21 +24,36 @@ void PostProcessing::computeRelativeAngleYHeadToTorso() {
glm::mat4 relativeTransformation = realHeadTransform
* glm::inverse(realTorsoTransform);
relativeAngleYHeadTorso = angleAroundGlobalY(glm::mat3(relativeTransformation));
relativeAngleYHeadTorsoDeg = glm::degrees(angleAroundGlobalY(glm::mat3(relativeTransformation)));
}
}
void PostProcessing::computeAndAddAngleYHead() {
void PostProcessing::computeAndAddAnglesHead() {
if (frames > 0 && hasValidHeadTransform && lastHeadTransformValid) {
glm::mat4 relativeHeadTransformFrame = realHeadTransform *
glm::inverse(realHeadTransformLast);
angleYHeadCurrentFrame = angleAroundGlobalY(glm::mat3(relativeHeadTransformFrame));
accumulatedAngleYHead += angleYHeadCurrentFrame;
glm::mat4 relativeHeadTransformFrameVirtual = virtualHeadTransform *
glm::inverse(virtualHeadTransformLast);
angleYHeadCurrentFrameDeg = glm::degrees(angleAroundGlobalY(glm::mat3(relativeHeadTransformFrame)));
accumulatedAngleYHeadDeg += angleYHeadCurrentFrameDeg;
angleYHeadCurrentFrameVirtualDeg = glm::degrees(angleAroundGlobalY(glm::mat3(relativeHeadTransformFrameVirtual)));
accumulatedAngleYHeadVirtualDeg += angleYHeadCurrentFrameVirtualDeg;
angleXHeadCurrentFrameDeg = glm::degrees(angleAroundLocalX(glm::mat3(relativeHeadTransformFrame)));
angleXHeadDeg = glm::degrees(angleAroundLocalX(glm::mat3(realHeadTransform)));
assert(!isnanf(angleXHeadCurrentFrameDeg));
assert(!isnanf(angleXHeadDeg));
angleXHeadTotalDeg += fabs(angleXHeadCurrentFrameDeg);
} else {
angleYHeadCurrentFrame = 0.;
angleYHeadCurrentFrameDeg = 0.;
angleXHeadCurrentFrameDeg = 0.;
angleYHeadCurrentFrameVirtualDeg = 0.;
}
}
......@@ -48,18 +63,28 @@ void PostProcessing::computeAndAddAngleYTorso() {
glm::mat4 relativeTorsoTransformFrame = realTorsoTransform *
glm::inverse(realTorsoTransformLast);
angleYTorsoCurrentFrame = angleAroundGlobalY(glm::mat3(relativeTorsoTransformFrame));
accumulatedAngleYTorso += angleYTorsoCurrentFrame;
angleYTorsoCurrentFrameDeg = glm::degrees(angleAroundGlobalY(glm::mat3(relativeTorsoTransformFrame)));
accumulatedAngleYTorsoDeg += angleYTorsoCurrentFrameDeg;
} else {
angleYTorsoCurrentFrame = 0.;
angleYTorsoCurrentFrameDeg = 0.;
}
}
void PostProcessing::computeAndAddAngleYTorsoVirtual() {
if (hasValidTorsoTransform && hasValidHeadTransform) {
accumulatedAngleYTorsoVirtualDeg = accumulatedAngleYHeadVirtualDeg - relativeAngleYHeadTorsoDeg;
}
}
void PostProcessing::checkForValidTransforms() {
if(realHeadTransform[3] == realHeadTransformLast[3]) {
hasValidHeadTransform = false;
} else {
hasValidHeadTransform = false;
if((frames > 0) && (realHeadTransform[3] != realHeadTransformLast[3])) {
hasValidHeadTransform = true;
}
......@@ -76,46 +101,64 @@ void PostProcessing::checkForValidTransforms() {
if(frames < 2) {
hasBodyTorsoFront = false;
hasBodyTorsoBack = false;
}
} else {
hasValidTorsoTransform = (
(hasBodyTorsoBack && hasBodyTorsoFront) ||
((hasBodyTorsoBack || hasBodyTorsoFront) && hasValidTorsoOffsets)
);
hasValidTorsoTransform = (
(hasBodyTorsoBack && hasBodyTorsoFront) ||
((hasBodyTorsoBack || hasBodyTorsoFront) && hasValidTorsoOffsets)
);
}
}
std::string PostProcessing::GetCleanedStringFromDataLine(std::string line) {
std::istringstream ssLine(line);
std::string token;
std::getline(ssLine, token, ',');
std::getline(ssLine, token, ',');
std::getline(ssLine, token, ',');
std::getline(ssLine, token, ',');
std::getline(ssLine, token, ',');
std::getline(ssLine, token, ','); //"KINEMATIC"
std::string eventString = token;
std::getline(ssLine, token, ','); //framenumber
std::getline(ssLine, token, ','); //timestamp
std::string stringTimestamp = token;
std::getline(ssLine, token, ','); //gain
std::string stringGain = token;
std::getline(ssLine, token, ','); //matrix (real head)
realHeadTransform = ReadMat4(token);
std::getline(ssLine, token, ',');
std::getline(ssLine, token, ',');
std::getline(ssLine, token, ',');
std::getline(ssLine, token, ','); //matrix (virtual head)
virtualHeadTransform = ReadMat4(token);
std::getline(ssLine, token, ','); //nothing
std::getline(ssLine, token, ','); //matrix (real front)
realFrontTransform = ReadMat4(token);
realFrontTransform = correctBodyTransform(realFrontTransform);
std::getline(ssLine, token, ',');
std::getline(ssLine, token, ','); //matrix (real back)
realBackTransform = ReadMat4(token);
realBackTransform = correctBodyTransform(realBackTransform);
std::getline(ssLine, token, ',');
std::string stringTotalAngleYReal = token;
std::getline(ssLine, token, ',');
std::string stringTotalAngleYVirtual = token;
std::getline(ssLine, token, ',');
std::string stringTotalDistance = token;
checkForValidTransforms();
computeTorsoTransform();
computeRelativeAngleYHeadToTorso();
computeAndAddAngleYHead();
computeAndAddAnglesHead();
computeAndAddAngleYTorso();
computeAndAddAngleYTorsoVirtual();
#if DEBUG_OUTPUT
std::cout << "+++++++++++++++++++++++++++++++" << std::endl;
......@@ -148,25 +191,39 @@ std::string PostProcessing::GetCleanedStringFromDataLine(std::string line) {
std::string torsoTransformString = "UNDEFINED";
std::string relativeAngleYString = "UNDEFINED";
std::string virtualTorsoAngleYString = "UNDEFINED";
if(hasValidTorsoTransform) {
torsoTransformString = getMatlabString(realTorsoTransform);
//relativeAngleString = std::to_string(relativeAngleHeadTorso);
if (hasValidHeadTransform) {
relativeAngleYString = R2S(relativeAngleYHeadTorso);
relativeAngleYString = R2S(relativeAngleYHeadTorsoDeg);
virtualTorsoAngleYString = R2S(accumulatedAngleYTorsoVirtualDeg);
}
}
lineOut =
getMatlabString(realHeadTransform) + "," +
getMatlabString(realFrontTransform) + "," +
getMatlabString(realBackTransform) + "," +
torsoTransformString + "," +
R2S(angleYHeadCurrentFrame) + "," +
R2S(angleYTorsoCurrentFrame) + "," +
R2S(accumulatedAngleYHead) + "," +
R2S(accumulatedAngleYTorso) + "," +
relativeAngleYString + ",";
//stringTimestamp + "," + //OK
//stringGain + "," + //OK
//getMatlabStringPositionXnegativeZWithOffsetCorrection(realHeadTransform) + "," + //OK
//getMatlabStringPositionXnegativeZWithOffsetCorrection(virtualHeadTransform) + "," + //OK
//R2S(angleXHeadDeg) + "," + //OK
//R2S(angleXHeadTotalDeg) + "," + //OK
R2S(accumulatedAngleYHeadDeg) + "," +
R2S(accumulatedAngleYTorsoDeg) + "," +
R2S(accumulatedAngleYHeadVirtualDeg) + "," +
virtualTorsoAngleYString + "," +
relativeAngleYString + "," +
stringTotalDistance + "," + // Reihenfolge
stringTotalAngleYReal + "," + // falsch
stringTotalAngleYVirtual // gewesen ist.
;
}
......@@ -174,6 +231,7 @@ std::string PostProcessing::GetCleanedStringFromDataLine(std::string line) {
lastTorsoTransformValid = hasValidTorsoTransform;
lastHeadTransformValid = hasValidHeadTransform;
realHeadTransformLast = realHeadTransform;
virtualHeadTransformLast = virtualHeadTransform;
realFrontTransformLast = realFrontTransform;
realBackTransformLast = realBackTransform;
realTorsoTransformLast = realTorsoTransform;
......@@ -193,6 +251,16 @@ std::string PostProcessing::getMatlabString(glm::mat4 v) {
return stringStream.str();
}
std::string PostProcessing::getMatlabStringPositionXnegativeZWithOffsetCorrection(glm::mat4 v) {
std::stringstream stringStream;
stringStream << std::setprecision(std::numeric_limits<double>::digits10 + 1)
<< "[ "
<< v[3][0] - 4.0f << "; "
<< -v[3][2] - 4.0f << "] ";
return stringStream.str();
//offset is (-4.f, 0.f, 4.f);
}
void PostProcessing::computeTorsoTransform() {
glm::vec3 realTorsoPosition;
glm::mat4 realTorsoOrientation;
......@@ -375,4 +443,14 @@ glm::mat4 PostProcessing::ReadMat4(std::string strMatrix) {
}
return mat;
}
\ No newline at end of file
}
float PostProcessing::ReadFloat(std::string strFloat) {
float f;
std::istringstream ssFloat(strFloat);
ssFloat >> std::setprecision(std::numeric_limits<double>::digits10 + 1)
>> f;
return f;
}
......@@ -18,6 +18,7 @@ private:
//Update if there is new data:
glm::mat4 realHeadTransform;
glm::mat4 virtualHeadTransform;
glm::mat4 realFrontTransform;
glm::mat4 realBackTransform;
glm::mat4 realTorsoTransform;
......@@ -27,6 +28,7 @@ private:
//Update each frame:
glm::mat4 realHeadTransformLast;
glm::mat4 virtualHeadTransformLast;
glm::mat4 realFrontTransformLast;
glm::mat4 realBackTransformLast;
glm::mat4 realTorsoTransformLast;
......@@ -40,20 +42,31 @@ private:
bool hasBodyTorsoFront;
bool hasBodyTorsoBack;
float relativeAngleYHeadTorso;
float angleYHeadCurrentFrame;
float angleYTorsoCurrentFrame;
float accumulatedAngleYHead;
float accumulatedAngleYTorso;
float relativeAngleYHeadTorsoDeg;
float angleYHeadCurrentFrameDeg;
float angleXHeadCurrentFrameDeg;
float angleYTorsoCurrentFrameDeg;
float accumulatedAngleYHeadDeg = 0.0f;
float angleXHeadDeg = 0.0f;
float accumulatedAngleYTorsoDeg = 0.0f;
float angleXHeadTotalDeg = 0.0f;
float angleYHeadCurrentFrameVirtualDeg;
float angleYTorsoCurrentFrameVirtualDeg;
float accumulatedAngleYHeadVirtualDeg = 0.0f;
float accumulatedAngleYTorsoVirtualDeg = 0.0f;
void checkForValidTransforms();
void computeRelativeAngleYHeadToTorso();
void computeAndAddAngleYHead();
void computeAndAddAnglesHead();
void computeAndAddAngleYTorso(); //Call AFTER torso transform is calculated
//void computeAndAddAngleXHead();
void computeAndAddAngleYTorsoVirtual();
std::string getMatlabString(glm::mat4 v);
std::string getMatlabStringPositionXnegativeZWithOffsetCorrection(glm::mat4 v);
void computeTorsoTransform();
......@@ -73,6 +86,7 @@ private:
glm::mat4 cleanMatrix(glm::mat4 matrix);
glm::mat4 ReadMat4(std::string strMatrix);
float ReadFloat(std::string strFloat);
std::string R2S(float f);
......
......@@ -417,7 +417,7 @@ void TestScene::ReceivePackage(JNIEnv *env, void *buffer, int length)
}
}
Log(
LogError(
dataCleanTest.GetCleanedStringFromDataLine(
dataCapture.GetKinematicDataString(frameNr, userKinematic,
torsoTransforms, totalRealRotation,
......
......@@ -27,6 +27,9 @@ glm::vec3 offsetTorsoBackToCenter;
glm::mat4 correctBodyTransform(glm::mat4 transform);
glm::mat4 cleanMatrix(glm::mat4 matrix);
DataReader reader;
bool hasValidTorsoTransform; //false, if torso transformation is UNDEFINED
float relativeAngleHeadTorso;
......@@ -107,88 +110,95 @@ void computeRelativeAngleHeadToTorso() {
}
std::string getCleanedStringFromDataLine(std::string line) {
std::istringstream ssLine(line);
void insertTorsoTransformAndRelativeAngleAndCleanData()
{
std::ifstream file( "testfile" );
std::ofstream fileOut( "testfile.out" );
std::string token;
std::getline(ssLine, token, ',');
std::getline(ssLine, token, ',');
std::getline(ssLine, token, ',');
std::getline(ssLine, token, ',');
std::getline(ssLine, token, ',');
if(file) {
DataReader reader;
realHeadTransform = reader.ReadMat4(token);
std::string line;
while(std::getline(file, line)) {
std::istringstream ssLine(line);
std::string token;
std::getline(ssLine, token, ',');
std::getline(ssLine, token, ',');
std::getline(ssLine, token, ',');
std::getline(ssLine, token, ',');
std::getline(ssLine, token, ',');
realHeadTransform = reader.ReadMat4(token);
if(realHeadTransform[3] == realHeadTransformLast[3])
return "";
if(realHeadTransform[3] == realHeadTransformLast[3])
continue;
std::getline(ssLine, token, ',');
std::getline(ssLine, token, ',');
std::getline(ssLine, token, ',');
std::getline(ssLine, token, ',');
std::getline(ssLine, token, ',');
std::getline(ssLine, token, ',');
realFrontTransform = reader.ReadMat4(token);
realFrontTransform = correctBodyTransform(realFrontTransform);
realFrontTransform = reader.ReadMat4(token);
realFrontTransform = correctBodyTransform(realFrontTransform);
std::getline(ssLine, token, ',');
std::getline(ssLine, token, ',');
realBackTransform = reader.ReadMat4(token);
realBackTransform = correctBodyTransform(realBackTransform);
realBackTransform = reader.ReadMat4(token);
realBackTransform = correctBodyTransform(realBackTransform);
#if DEBUG_OUTPUT
std::cout << "+++++++++++++++++++++++++++++++" << std::endl;
std::cout << "realHeadTransform" << std::endl;
std::cout << glm::to_string(realHeadTransform) << std::endl;
std::cout << "+++++++++++++++++++++++++++++++" << std::endl;
std::cout << "realHeadTransform" << std::endl;
std::cout << glm::to_string(realHeadTransform) << std::endl;
std::cout << "realFrontTransform" << std::endl;
std::cout << glm::to_string(realFrontTransform) << std::endl;
std::cout << "realFrontTransform" << std::endl;
std::cout << glm::to_string(realFrontTransform) << std::endl;
std::cout << "realBackTransform" << std::endl;
std::cout << glm::to_string(realBackTransform) << std::endl;
std::cout << "realBackTransform" << std::endl;
std::cout << glm::to_string(realBackTransform) << std::endl;
#endif
computeTorsoTransform();
computeTorsoTransform();
#if DEBUG_OUTPUT
std::cout << "realTorsoTransform" << std::endl;
std::cout << glm::to_string(realTorsoTransform) << std::endl;
std::cout << "realTorsoTransform" << std::endl;
std::cout << glm::to_string(realTorsoTransform) << std::endl;
#endif
computeRelativeAngleHeadToTorso();
computeRelativeAngleHeadToTorso();
#if DEBUG_OUTPUT
std::cout << "relativeAngleHeadTorso" << std::endl;
std::cout << relativeAngleHeadTorso << std::endl;
std::cout << "relativeAngleHeadTorso" << std::endl;
std::cout << relativeAngleHeadTorso << std::endl;
#endif
realHeadTransformLast = realHeadTransform;
realFrontTransformLast = realFrontTransform;
realBackTransformLast = realBackTransform;
realHeadTransformLast = realHeadTransform;
realFrontTransformLast = realFrontTransform;
realBackTransformLast = realBackTransform;
std::string torsoTransformString = "UNDEFINED";
std::string relativeAngleString = "UNDEFINED";
if(hasValidTorsoTransform) {
torsoTransformString = getMatlabString(realTorsoTransform);
relativeAngleString = std::to_string(relativeAngleHeadTorso);
}
std::string torsoTransformString = "UNDEFINED";
std::string relativeAngleString = "UNDEFINED";
if(hasValidTorsoTransform) {
torsoTransformString = getMatlabString(realTorsoTransform);
relativeAngleString = std::to_string(relativeAngleHeadTorso);
}
std::string lineOut =
getMatlabString(realHeadTransform) + "," +
getMatlabString(realFrontTransform) + "," +
getMatlabString(realBackTransform) + "," +
torsoTransformString + "," +
relativeAngleString + ",";
fileOut << lineOut << std::endl;
std::string lineOut =
getMatlabString(realHeadTransform) + "," +
getMatlabString(realFrontTransform) + "," +
getMatlabString(realBackTransform) + "," +
torsoTransformString + "," +
relativeAngleString + ",";
return lineOut;
}
void insertTorsoTransformAndRelativeAngleAndCleanData()
{
std::ifstream file( "testfile" );
std::ofstream fileOut( "testfile.out" );
if(file) {
std::string line;
while(std::getline(file, line)) {
std::string lineOut = getCleanedStringFromDataLine(line);
if (lineOut.length() > 0) {
fileOut << lineOut << std::endl;
}
}
file.close();
......@@ -222,7 +232,7 @@ void computeTorsoTransform() {
hasBodyTorsoBack = true;
// instead skip first line in main loop?
static bool skippedFrames = 0;
static unsigned int skippedFrames = 0;
if(skippedFrames < 2) {
hasBodyTorsoFront = false;
hasBodyTorsoBack = false;
......
......@@ -15,6 +15,8 @@ add_executable(${PROJECT}
TcpReceiver.cpp
UdpSender.cpp
ViewerUDPStreamer.cpp
DataCaptureManager.cpp
datareader.cpp
)
target_include_directories(${PROJECT} PUBLIC
......
This diff is collapsed.
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