Commit 3226c0b7 authored by Kaspar Scharf's avatar Kaspar Scharf

PostProcessing: Added calculation of relative angle head-torso (Not tested for validity yet).

parent 8324c512
......@@ -27,9 +27,15 @@ glm::vec3 offsetTorsoBackToCenter;
glm::mat4 correctBodyTransform(glm::mat4 transform);
glm::mat4 cleanMatrix(glm::mat4 matrix);
bool hasValidTorsoTransform; //false, if torso transformation is UNDEFINED
float relativeAngleHeadTorso;
void cutOffLinesAfterLastTargetCollected();
void insertTorsoTransformAndRelativeAngleAndCleanData();
void computeTorsoTransform();
void handleBodyFront(glm::vec3 & realTorsoPosition,
glm::mat4 & realTorsoOrientation);
......@@ -38,11 +44,14 @@ void handleBodyBack(glm::vec3 & realTorsoPosition,
void handleBodyBoth(glm::vec3 & realTorsoPosition,
glm::mat4 & realTorsoOrientation);
float angleAroundGlobalY(glm::mat3 transform);
std::string getMatlabString(glm::mat4 v);
int main() {
cutOffLinesAfterLastTargetCollected();
//cutOffLinesAfterLastTargetCollected();
insertTorsoTransformAndRelativeAngleAndCleanData();
return 0;
}
......@@ -87,7 +96,20 @@ void cutOffLinesAfterLastTargetCollected() {
void insertTorsoTransformAndCleanData()
void computeRelativeAngleHeadToTorso() {
relativeAngleHeadTorso = -1.; // -1 represents UNDEFINED
if (hasValidTorsoTransform) {
glm::mat4 relativeTransformation = realHeadTransform
* glm::inverse(realTorsoTransform);
relativeAngleHeadTorso = angleAroundGlobalY(glm::mat3(relativeTransformation));
}
}
void insertTorsoTransformAndRelativeAngleAndCleanData()
{
std::ifstream file( "testfile" );
std::ofstream fileOut( "testfile.out" );
......@@ -143,6 +165,13 @@ void insertTorsoTransformAndCleanData()
std::cout << glm::to_string(realTorsoTransform) << std::endl;
#endif
computeRelativeAngleHeadToTorso();
#if DEBUG_OUTPUT
std::cout << "relativeAngleHeadTorso" << std::endl;
std::cout << relativeAngleHeadTorso << std::endl;
#endif
realHeadTransformLast = realHeadTransform;
realFrontTransformLast = realFrontTransform;
realBackTransformLast = realBackTransform;
......@@ -151,7 +180,8 @@ void insertTorsoTransformAndCleanData()
getMatlabString(realHeadTransform) + "," +
getMatlabString(realFrontTransform) + "," +
getMatlabString(realBackTransform) + "," +
getMatlabString(realTorsoTransform) + ",";
getMatlabString(realTorsoTransform) + "," +
std::to_string(relativeAngleHeadTorso) + ",";
fileOut << lineOut << std::endl;
}
......@@ -191,51 +221,69 @@ void computeTorsoTransform() {
hasBodyTorsoFront = false;
hasBodyTorsoBack = false;
firstFrame = false;
hasValidTorsoTransform = true;
} else {
if (!hasBodyTorsoBack && !hasBodyTorsoFront) {
hasValidTorsoTransform = false;
} else {
hasValidTorsoTransform = true;
}
}
if(!hasBodyTorsoBack && hasBodyTorsoFront) {
handleBodyFront(realTorsoPosition, realTorsoOrientation);
}
else if(!hasBodyTorsoFront && hasBodyTorsoBack) {
handleBodyBack(realTorsoPosition, realTorsoOrientation);
}
else if(hasBodyTorsoFront && hasBodyTorsoBack) {
handleBodyBoth(realTorsoPosition, realTorsoOrientation);
}
#if DEBUG_OUTPUT
std::cout << std::boolalpha
<< "hasValidTorsoTransform: " << hasValidTorsoTransform << std::endl;
#endif
if (hasBodyTorsoFront) {
offsetTorsoFrontToCenter =
glm::transpose(glm::mat3(realFrontTransform)) *
(realTorsoPosition - glm::vec3(realFrontTransform[3]));
}
if (hasValidTorsoTransform) {
//If no torso body is tracked for the current frame, skip the calculation.
if (hasBodyTorsoBack) {
offsetTorsoBackToCenter =
glm::transpose(glm::mat3(realBackTransform)) *
(realTorsoPosition - glm::vec3(realBackTransform[3]));
}
if(!hasBodyTorsoBack && hasBodyTorsoFront) {
handleBodyFront(realTorsoPosition, realTorsoOrientation);
}
else if(!hasBodyTorsoFront && hasBodyTorsoBack) {
handleBodyBack(realTorsoPosition, realTorsoOrientation);
}
else if(hasBodyTorsoFront && hasBodyTorsoBack) {
handleBodyBoth(realTorsoPosition, realTorsoOrientation);
}
#if DEBUG_OUTPUT
std::cout << std::boolalpha
<< "hasBodyTorsoFront: " << hasBodyTorsoFront << std::endl;
std::cout << std::boolalpha
<< "hasBodyTorsoBack: " << hasBodyTorsoBack << std::endl;
if (hasBodyTorsoFront) {
offsetTorsoFrontToCenter =
glm::transpose(glm::mat3(realFrontTransform)) *
(realTorsoPosition - glm::vec3(realFrontTransform[3]));
}
std::cout << "offsetTorsoFrontToCenter: "
<< glm::to_string(offsetTorsoFrontToCenter) << std::endl;
std::cout << "offsetTorsoBackToCenter: "
<< glm::to_string(offsetTorsoBackToCenter) << std::endl;
if (hasBodyTorsoBack) {
offsetTorsoBackToCenter =
glm::transpose(glm::mat3(realBackTransform)) *
(realTorsoPosition - glm::vec3(realBackTransform[3]));
}
std::cout << "realTorsoPosition: "
<< glm::to_string(realTorsoPosition) << std::endl;
#if DEBUG_OUTPUT
std::cout << std::boolalpha
<< "hasBodyTorsoFront: " << hasBodyTorsoFront << std::endl;
std::cout << std::boolalpha
<< "hasBodyTorsoBack: " << hasBodyTorsoBack << std::endl;
std::cout << "offsetTorsoFrontToCenter: "
<< glm::to_string(offsetTorsoFrontToCenter) << std::endl;
std::cout << "offsetTorsoBackToCenter: "
<< glm::to_string(offsetTorsoBackToCenter) << std::endl;
std::cout << "realTorsoPosition: "
<< glm::to_string(realTorsoPosition) << std::endl;
#endif
realTorsoTransform =
glm::translate(glm::mat4(1.0f), realTorsoPosition) *
realTorsoOrientation;
realTorsoTransform =
glm::translate(glm::mat4(1.0f), realTorsoPosition) *
realTorsoOrientation;
for(auto & element : realTorsoTransform)
assert(glm::isnan(element) == false);
}
for(auto & element : realTorsoTransform)
assert(glm::isnan(element) == false);
}
void handleBodyFront(glm::vec3 & realTorsoPosition,
......
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